CloudViewer: fixed build with vtk<6

* Added Kinect for Azure menu option

* Reintegrated Kinect for Azure initialization code, fixed previous GUI commit

* Added ifdefs to fix build

* Added preliminary K4A device support

* Edits to K4A camera implementation

* K4A minor initialization fix

* Typo

* Another initialization fix

* Update CameraRGBD tool to display K4A camera output

* K4A added model parameter so we can create maps

CMakeLists.txt

@@ -20,8 +20,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 19)
-SET(RTABMAP_PATCH_VERSION 7)
+SET(RTABMAP_MINOR_VERSION 20)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -163,7 +163,8 @@ ELSE()
 option(WITH_QT            "Include Qt support"                   ON)
 ENDIF()
 option(WITH_ORB_OCTREE    "Include ORB Octree feature support"   ON)
-option(WITH_SP_TORCH      "Include SuperPoint Torch feature support"   ON)
+option(WITH_SUPERPOINT_TORCH  "Include SuperPoint Torch feature support"   ON)
+option(WITH_PYMATCHER     "Include Python3 matchers support"     OFF)
 option(WITH_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_K4W2          "Include Kinect for Windows v2 support" ON)
@@ -321,12 +322,19 @@ IF(WITH_QT)
     ENDIF(QT4_FOUND OR Qt5_FOUND)
 ENDIF(WITH_QT)
 
-IF(WITH_SP_TORCH)
+IF(WITH_SUPERPOINT_TORCH)
     FIND_PACKAGE(Torch QUIET)
     IF(TORCH_FOUND)
         MESSAGE(STATUS "Found Torch: ${TORCH_INCLUDE_DIRS}")
     ENDIF(TORCH_FOUND)
-ENDIF(WITH_SP_TORCH)
+ENDIF(WITH_SUPERPOINT_TORCH)
+
+IF(WITH_PYMATCHER)
+    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development)
+    IF(Python3_FOUND)
+        MESSAGE(STATUS "Found Python3")
+    ENDIF(Python3_FOUND)
+ENDIF(WITH_PYMATCHER)
 
 IF(WITH_FREENECT)
     FIND_PACKAGE(Freenect QUIET)
@@ -592,7 +600,7 @@ IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
     ENDIF(ORB_SLAM2_FOUND)
 ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
 
-IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1")
+IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND)
    #LOAM and PCL>=1.10 require c++14
    IF(NOT MSVC)
      include(CheckCXXCompilerFlag)
@@ -664,6 +672,10 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
 SET(CONF_DEPENDENCIES 
     ${ZLIB_LIBRARIES}
 )
+
+# OpenCV2 has nonfree if OPENCV_NONFREE_FOUND
+# OpenCV<=3.4.2 has nonfree if OPENCV_XFEATURES2D_FOUND
+# OpenCV>3.4.2 has nonfree if OPENCV_XFEATURES2D_FOUND and OPENCV_ENABLE_NONFREE is defined
 IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
    SET(NONFREE "//")
 ELSEIF(OpenCV_VERSION VERSION_GREATER "3.4.2")
@@ -677,6 +689,16 @@ ELSEIF(OpenCV_VERSION VERSION_GREATER "3.4.2")
   ENDIF(${matchres} EQUAL -1)
 ENDIF()
 
+# check if version status is "-dev" (SIFT compatibility issue between 4.3.0 vs 4.3.0-dev)
+FIND_FILE(OpenCV_VERSION_HPP opencv2/core/version.hpp
+  PATHS ${OpenCV_INCLUDE_DIRS}
+  NO_DEFAULT_PATH)
+FILE(READ ${OpenCV_VERSION_HPP} TMPTXT)
+STRING(FIND "${TMPTXT}" "-dev" matchres)
+IF(${matchres} EQUAL -1)
+  SET(OPENCV_DEV "//")
+ENDIF(${matchres} EQUAL -1)
+
 IF(NOT G2O_FOUND)
    SET(G2O "//")
 ELSE()
@@ -826,7 +848,10 @@ IF(NOT WITH_ORB_OCTREE)
    SET(ORB_OCTREE "//")
 ENDIF()
 IF(NOT TORCH_FOUND)
-   SET(SP_TORCH "//")
+   SET(SUPERPOINT_TORCH "//")
+ENDIF()
+IF(NOT Python3_FOUND)
+   SET(PYMATCHER "//")
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
    SET(CONF_VTK_QT true)
@@ -841,9 +866,6 @@ IF(NOT WITH_MADGWICK)
    SET(MADGWICK "//")
 ENDIF()
 
-IF(NOT (OpenCV_FOUND AND NOT (OpenCV_VERSION_MAJOR LESS 3)))
-   SET(OPENCV3 "//")
-ENDIF(NOT (OpenCV_FOUND AND NOT (OpenCV_VERSION_MAJOR LESS 3)))
 CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)
 
 ADD_SUBDIRECTORY( utilite )
@@ -1046,9 +1068,13 @@ IF(OpenCV_FOUND)
     ENDIF()
   ELSE()
     IF(OPENCV_XFEATURES2D_FOUND)
-      MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
+      IF(NONFREE STREQUAL "//")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: BSD)")
+      ELSE()
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES (License: Non commercial)")
+      ENDIF()
     ELSE()
-      MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: BSD)")
     ENDIF()
   ENDIF()
 ENDIF(OpenCV_FOUND)
@@ -1057,7 +1083,7 @@ IF(QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
 ELSEIF(Qt5_FOUND)
-MESSAGE(STATUS "  With Qt5                  = YES (License: Open Source or Commercial)")
+MESSAGE(STATUS "  With Qt ${Qt5_VERSION}            = YES (License: Open Source or Commercial)")
 MESSAGE(STATUS "  With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}              = YES (License: BSD)")
 
 ELSEIF(NOT WITH_QT)
@@ -1079,11 +1105,19 @@ MESSAGE(STATUS "  With ORB OcTree           = NO (WITH_ORB_OCTREE=OFF)")
 ENDIF()
 
 IF(TORCH_FOUND)
-MESSAGE(STATUS "  With SupertPoint Torch    = YES (License: GPLv3) libtorch=${Torch_VERSION}")
-ELSEIF(NOT WITH_SP_TORCH)
-MESSAGE(STATUS "  With SupertPoint Torch    = NO (WITH_SP_TORCH=OFF)")
+MESSAGE(STATUS "  With SupertPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
+ELSEIF(NOT WITH_SUPERPOINT_TORCH)
+MESSAGE(STATUS "  With SupertPoint          = NO (WITH_SUPERPOINT_TORCH=OFF)")
 ELSE()
-MESSAGE(STATUS "  With SupertPoint Torch    = NO (libtorch not found)")
+MESSAGE(STATUS "  With SupertPoint          = NO (libtorch not found)")
+ENDIF()
+
+IF(Python3_FOUND)
+MESSAGE(STATUS "  With Python3              = YES (License: PSF)")
+ELSEIF(NOT WITH_PYMATCHER)
+MESSAGE(STATUS "  With Python3              = NO (WITH_PYMATCHER=OFF)")
+ELSE()
+MESSAGE(STATUS "  With Python3              = NO (python3 not found)")
 ENDIF()
 
 IF(WITH_MADGWICK)

Version.h.in

@@ -38,13 +38,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define RTABMAP_VERSION_COMPARE(major, minor, patch)  (major>=@PROJECT_VERSION_MAJOR@ || (major==@PROJECT_VERSION_MAJOR@ && minor>=@PROJECT_VERSION_MINOR@) || (major==@PROJECT_VERSION_MAJOR@ && minor==@PROJECT_VERSION_MINOR@ && patch >=@PROJECT_VERSION_PATCH@))
 
 @NONFREE@#define RTABMAP_NONFREE
+@OPENCV_DEV@#define RTABMAP_OPENCV_DEV
 @TORO@#define RTABMAP_TORO
 @G2O@#define RTABMAP_G2O
 @G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
 @GTSAM@#define RTABMAP_GTSAM
 @CERES@#define RTABMAP_CERES
 @VERTIGO@#define RTABMAP_VERTIGO
-@OPENCV3@#define RTABMAP_OPENCV3
 @OPENNI2@#define RTABMAP_OPENNI2
 @FREENECT@#define RTABMAP_FREENECT
 @FREENECT2@#define RTABMAP_FREENECT2
@@ -73,7 +73,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @VINS@#define RTABMAP_VINS
 @ORB_SLAM2@#define RTABMAP_ORB_SLAM2
 @ORB_OCTREE@#define RTABMAP_ORB_OCTREE
-@SP_TORCH@#define RTABMAP_SP_TORCH
+@SUPERPOINT_TORCH@#define RTABMAP_SUPERPOINT_TORCH
+@PYMATCHER@#define RTABMAP_PYMATCHER
 @MADGWICK@#define RTABMAP_MADGWICK
 
 

app/android/jni/RTABMapApp.cpp

@@ -398,11 +398,15 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	std::multimap<int, rtabmap::Link> links;
 	LOGI("Loading full map from database...");
 	UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading data from database..."));
-	rtabmap_->get3DMap(
-			signatures,
+	rtabmap_->getGraph(
 			poses,
 			links,
 			true,
+			true,
+			&signatures,
+			true,
+			true,
+			true,
 			true);
 
 	if(signatures.size() && poses.empty())
@@ -2391,12 +2395,13 @@ bool RTABMapApp::exportMesh(
 							gains[1] = jter->second.gains[1];
 							gains[2] = jter->second.gains[2];
 
-							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, false);
+							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
 							data.uncompressData(0, &depth);
 						}
 						else
 						{
-							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
+							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
+							data.uncompressData();
 							if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
 							{
 								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get());
@@ -2661,7 +2666,8 @@ bool RTABMapApp::exportMesh(
 						}
 						else
 						{
-							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
+							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
+							data.uncompressData();
 							if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
 							{
 								cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_);
@@ -2894,7 +2900,8 @@ bool RTABMapApp::exportMesh(
 						gains[1] = jter->second.gains[1];
 						gains[2] = jter->second.gains[2];
 					}
-					rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
+					rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
+					data.uncompressData();
 					if(!data.imageRaw().empty() && !data.depthRaw().empty())
 					{
 						// full resolution
@@ -2913,7 +2920,8 @@ bool RTABMapApp::exportMesh(
 					}
 					else
 					{
-						rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
+						rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
+						data.uncompressData();
 						if(!data.imageRaw().empty() && !data.depthRaw().empty())
 						{
 							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get());

corelib/include/rtabmap/core/CameraModel.h

@@ -50,7 +50,7 @@ public:
 			const cv::Mat & D,
 			const cv::Mat & R,
 			const cv::Mat & P,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
 
 	// minimal
 	CameraModel(
@@ -58,7 +58,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 	// minimal to be saved
@@ -68,7 +68,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 
@@ -113,6 +113,9 @@ public:
 	int imageWidth() const {return imageSize_.width;}
 	int imageHeight() const {return imageSize_.height;}
 
+	double fovX() const {return imageSize_.width>0 && fx()>0?2.0*atan(imageSize_.width/(fx()*2.0)):0.0;}
+	double fovY() const {return imageSize_.height>0 && fy()>0?2.0*atan(imageSize_.height/(fy()*2.0)):0.0;}
+
 	bool load(const std::string & directory, const std::string & cameraName);
 	bool save(const std::string & directory) const;
 	std::vector<unsigned char> serialize() const;

corelib/include/rtabmap/core/DBDriver.h

@@ -100,7 +100,7 @@ public:
 
 public:
 	void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
-	void addStatistics(const Statistics & statistics) const;
+	void addStatistics(const Statistics & statistics, bool saveWmState) const;
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
 	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
@@ -238,7 +238,7 @@ protected:
 			int nodeId,
 			const LaserScan & scan) const = 0;
 
-	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
+	virtual void addStatisticsQuery(const Statistics & statistics, bool saveWmState) const = 0;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
 	virtual cv::Mat loadPreviewImageQuery() const = 0;
 	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -104,7 +104,7 @@ protected:
 			int nodeId,
 			const LaserScan & scan) const;
 
-	virtual void addStatisticsQuery(const Statistics & statistics) const;
+	virtual void addStatisticsQuery(const Statistics & statistics, bool saveWmState) const;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const;
 	virtual cv::Mat loadPreviewImageQuery() const;
 	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
@@ -160,6 +160,7 @@ private:
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
+	std::string queryStepGlobalDescriptor() const;
 	std::string queryStepOccupancyGridUpdate() const;
 	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
 	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
@@ -169,7 +170,8 @@ private:
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
-	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepKeypoint(sqlite3_stmt * ppStmt, int nodeID, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepGlobalDescriptor(sqlite3_stmt * ppStmt, int nodeId, const GlobalDescriptor & descriptor) const;
 	void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
 			int nodeId,
 			const cv::Mat & ground,

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -75,8 +75,8 @@ public:
 	static cv::Mat findFFromWords(
 			const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
 			std::vector<uchar> & status,
-			double ransacParam1 = 3.0,
-			double ransacParam2 = 0.99);
+			double ransacReprojThreshold = 3.0,
+			double ransacConfidence = 0.99);
 
 	// assume a canonical camera (without K)
 	static void findRTFromP(

corelib/include/rtabmap/core/Features2d.h

@@ -62,7 +62,9 @@ namespace cv{
 namespace xfeatures2d {
 class FREAK;
 class BriefDescriptorExtractor;
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
 class SIFT;
+#endif
 class SURF;
 }
 namespace cuda {
@@ -71,7 +73,11 @@ class ORB;
 class SURF_CUDA;
 }
 }
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
 typedef cv::xfeatures2d::SIFT CV_SIFT;
+#else
+typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.3.0-dev
+#endif
 typedef cv::xfeatures2d::SURF CV_SURF;
 typedef cv::FastFeatureDetector CV_FAST;
 typedef cv::xfeatures2d::FREAK CV_FREAK;
@@ -110,6 +116,37 @@ public:
 		kFeatureKaze=9,     //new 0.13.2
 		kFeatureOrbOctree=10, //new 0.19.2
 		kFeatureSuperPointTorch=11}; //new 0.19.7
+	static std::string typeName(Type type)
+	{
+		switch(type){
+		case kFeatureSurf:
+			return "SURF";
+		case kFeatureSift:
+			return "SIFT";
+		case kFeatureOrb:
+			return "ORB";
+		case kFeatureFastFreak:
+			return "FAST+FREAK";
+		case kFeatureFastBrief:
+			return "FAST+BRIEF";
+		case kFeatureGfttFreak:
+			return "GFTT+Freak";
+		case kFeatureGfttBrief:
+			return "GFTT+Brief";
+		case kFeatureBrisk:
+			return "BRISK";
+		case kFeatureGfttOrb:
+			return "GFTT+ORB";
+		case kFeatureKaze:
+			return "KAZE";
+		case kFeatureOrbOctree:
+			return "ORB-OCTREE";
+		case kFeatureSuperPointTorch:
+			return "SUPERPOINT";
+		default:
+			return "Unknown";
+		}
+	}
 
 	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
 	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
@@ -146,6 +183,7 @@ public:
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
 	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols);
 
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
@@ -153,6 +191,8 @@ public:
 	int getMaxFeatures() const {return maxFeatures_;}
 	float getMinDepth() const {return _minDepth;}
 	float getMaxDepth() const {return _maxDepth;}
+	int getGridRows() const {return gridRows_;}
+	int getGridCols() const {return gridCols_;}
 
 public:
 	virtual ~Feature2D();
@@ -239,6 +279,7 @@ private:
 	double contrastThreshold_;
 	double edgeThreshold_;
 	double sigma_;
+	bool rootSIFT_;
 
 	cv::Ptr<CV_SIFT> _sift;
 };

corelib/include/rtabmap/core/GlobalDescriptor.h

@@ -0,0 +1,59 @@
+/*
+Copyright (c) 2010-2020, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+class GlobalDescriptor
+{
+
+public:
+	GlobalDescriptor(int type, const cv::Mat & data, const cv::Mat & info = cv::Mat()) :
+		type_(type),
+		info_(info),
+		data_(data)
+	{}
+	GlobalDescriptor() :
+		type_(-1) // Not set
+	{}
+	virtual ~GlobalDescriptor() {}
+
+	int type() const {return type_;}
+	const cv::Mat info() const {return info_;}
+	const cv::Mat data() const {return data_;}
+
+private:
+	int type_;
+	cv::Mat info_;
+	cv::Mat data_;
+};
+
+} // namespace rtabmap

corelib/include/rtabmap/core/Memory.h

@@ -93,7 +93,7 @@ public:
 	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
 
 	int cleanup();
-	void saveStatistics(const Statistics & statistics);
+	void saveStatistics(const Statistics & statistics, bool saveWMState);
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
 	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
@@ -197,16 +197,16 @@ public:
 			EnvSensors & sensors,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
-	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
-	void getNodeWords(int nodeId,
+	SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const;
+	void getNodeWordsAndGlobalDescriptors(int nodeId,
 			std::multimap<int, cv::KeyPoint> & words,
 			std::multimap<int, cv::Point3f> & words3,
-			std::multimap<int, cv::Mat> & wordsDescriptors);
+			std::multimap<int, cv::Mat> & wordsDescriptors,
+			std::vector<GlobalDescriptor> & globalDescriptors) const;
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
-			StereoCameraModel & stereoModel);
-	SensorData getSignatureDataConst(int locationId, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
-	std::set<int> getAllSignatureIds() const;
+			StereoCameraModel & stereoModel) const;
+	std::set<int> getAllSignatureIds(bool ignoreChildren = true) const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
 	bool isLocalizationDataSaved() const {return _localizationDataSaved;}
@@ -302,6 +302,7 @@ private:
 	bool _badSignaturesIgnored;
 	bool _mapLabelsAdded;
 	bool _depthAsMask;
+	bool _stereoFromMotion;
 	int _imagePreDecimation;
 	int _imagePostDecimation;
 	bool _compressionParallelized;

corelib/include/rtabmap/core/Odometry.h

@@ -78,6 +78,9 @@ public:
 	unsigned int framesProcessed() const {return framesProcessed_;}
 	bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}
 
+protected:
+	const std::map<double, Transform> & imus() const {return imus_;}
+
 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
 
@@ -90,7 +93,7 @@ private:
 	bool _force3DoF;
 	bool _holonomic;
 	bool guessFromMotion_;
-	bool guessSmoothingDelay_;
+	float guessSmoothingDelay_;
 	int _filteringStrategy;
 	int _particleSize;
 	float _particleNoiseT;
@@ -109,6 +112,7 @@ private:
 	double previousStamp_;
 	std::list<std::pair<std::vector<float>, double> > previousVelocities_;
 	Transform velocityGuess_;
+	Transform imuLastTransform_;
 	Transform previousGroundTruthPose_;
 	float distanceTravelled_;
 	unsigned int framesProcessed_;
@@ -116,6 +120,7 @@ private:
 	std::vector<ParticleFilter *> particleFilters_;
 	cv::KalmanFilter kalmanFilter_;
 	StereoCameraModel stereoModel_;
+	std::map<double, Transform> imus_;
 
 protected:
 	Odometry(const rtabmap::ParametersMap & parameters);

corelib/include/rtabmap/core/OdometryInfo.h

@@ -56,6 +56,8 @@ public:
 		interval(0),
 		distanceTravelled(0.0f),
 		memoryUsage(0),
+		gravityRollError(0.0),
+		gravityPitchError(0.0),
 		type(0)
 	{}
 
@@ -84,6 +86,8 @@ public:
 		output.guessVelocity = guessVelocity;
 		output.distanceTravelled = distanceTravelled;
 		output.memoryUsage = memoryUsage;
+		output.gravityRollError = gravityRollError;
+		output.gravityPitchError = gravityPitchError;
 		output.type = type;
 		return output;
 	}
@@ -110,6 +114,8 @@ public:
 	Transform guessVelocity;
 	float distanceTravelled;
 	int memoryUsage; //MB
+	double gravityRollError;
+	double gravityPitchError;
 
 	int type;
 

corelib/include/rtabmap/core/Parameters.h

@@ -32,6 +32,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/Version.h" // DLL export/import defines
 #include <rtabmap/utilite/UConversion.h>
+#include <opencv2/core/version.hpp>
+#include <opencv2/opencv_modules.hpp>
 #include <string>
 #include <map>
 
@@ -217,6 +219,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");
     RTABMAP_PARAM(Mem, InitWMWithAllNodes,          bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
     RTABMAP_PARAM(Mem, DepthAsMask,                 bool, true,     "Use depth image as mask when extracting features for vocabulary.");
+    RTABMAP_PARAM(Mem, StereoFromMotion,            bool, false,    uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str()));
     RTABMAP_PARAM(Mem, ImagePreDecimation,          int, 1,         "Image decimation (>=1) before features extraction.");
     RTABMAP_PARAM(Mem, ImagePostDecimation,         int, 1,         "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image.");
     RTABMAP_PARAM(Mem, CompressionParallelized,     bool, true,     "Compression of sensor data is multi-threaded.");
@@ -238,13 +241,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
     RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
     RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
-#ifndef RTABMAP_NONFREE
-#ifdef RTABMAP_OPENCV3
-    // OpenCV 3 without xFeatures2D module doesn't have BRIEF
+#if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
+    // OpenCV>2 without xFeatures2D module doesn't have BRIEF
     RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
-#else
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
-#endif
 #else
     RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
 #endif
@@ -280,6 +279,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(SIFT, ContrastThreshold, double, 0.04, "The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector.");
     RTABMAP_PARAM(SIFT, EdgeThreshold,     double, 10,   "The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).");
     RTABMAP_PARAM(SIFT, Sigma,             double, 1.6,  "The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.");
+    RTABMAP_PARAM(SIFT, RootSIFT,          bool,  false, "Apply RootSIFT normalization of the descriptors.");
 
     RTABMAP_PARAM(BRIEF, Bytes,            int, 32,      "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
 
@@ -324,11 +324,11 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(KAZE, NOctaveLayers,  int, 4,        "Default number of sublevels per scale level.");
     RTABMAP_PARAM(KAZE, Diffusivity,    int, 1,        "Diffusivity type: 0=DIFF_PM_G1, 1=DIFF_PM_G2, 2=DIFF_WEICKERT or 3=DIFF_CHARBONNIER.");
 
-    RTABMAP_PARAM_STR(SPTorch, ModelPath,              "", "[Required] Path to pre-trained weights Torch file of SuperPoint (*.pt).");
-    RTABMAP_PARAM(SPTorch, Threshold,         float,  0.200, "Detector response threshold to accept keypoint.");
-    RTABMAP_PARAM(SPTorch, NMS,               bool,  true, "If true, non-maximum suppression is applied to detected keypoints.");
-    RTABMAP_PARAM(SPTorch, MinDistance,       int,      4, uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSPTorchNMS().c_str()));
-    RTABMAP_PARAM(SPTorch, Cuda,              bool, false, "Use Cuda device for Torch, otherwise CPU device is used by default.");
+    RTABMAP_PARAM_STR(SuperPoint, ModelPath, "",           "[Required] Path to pre-trained weights Torch file of SuperPoint (*.pt).");
+    RTABMAP_PARAM(SuperPoint, Threshold,     float, 0.010, "Detector response threshold to accept keypoint.");
+    RTABMAP_PARAM(SuperPoint, NMS,           bool,  true,  "If true, non-maximum suppression is applied to detected keypoints.");
+    RTABMAP_PARAM(SuperPoint, NMSRadius,     int,  4,      uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointNMS().c_str()));
+    RTABMAP_PARAM(SuperPoint, Cuda,          bool, true,   "Use Cuda device for Torch, otherwise CPU device is used by default.");
 
     // BayesFilter
     RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9,  "Virtual place prior");
@@ -378,6 +378,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, ProximityPathMaxNeighbors,    int, 0,      "Maximum neighbor nodes compared on each path. Set to 0 to disable merging the laser scans.");
     RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,  "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
     RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for visual proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityOdomGuess,           bool, false, "Use odometry as motion guess for visual proximity detection.");
 
     // Graph optimization
 #ifdef RTABMAP_GTSAM
@@ -577,19 +578,15 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
 #endif
 
-    RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.1,    uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, MinInliers,               int, 20,       "Minimum feature correspondences to compute/accept the transformation.");
     RTABMAP_PARAM(Vis, MeanInliersDistance,      float, 0.0,    "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled.");
     RTABMAP_PARAM(Vis, MinInliersDistribution,   float, 0.0,    "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled.");
 
     RTABMAP_PARAM(Vis, Iterations,               int, 300,      "Maximum iterations to compute the transform.");
-#ifndef RTABMAP_NONFREE
-#ifdef RTABMAP_OPENCV3
-    // OpenCV 3 without xFeatures2D module doesn't have BRIEF
+#if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
+    // OpenCV>2 without xFeatures2D module doesn't have BRIEF
     RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
-#else
-    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
-#endif
 #else
     RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
 #endif
@@ -604,9 +601,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, GridRows,                 int, 1,      uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
     RTABMAP_PARAM(Vis, GridCols,                 int, 1,      uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
     RTABMAP_PARAM(Vis, CorType,                  int, 0,      "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
-    RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.6,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorCrossCheck,            bool, false,  uFormat("[%s=0] If true, brute force crosscheck matching is done instead of knn matching approach (%s).", kVisCorType().c_str(), kVisCorNNDR().c_str()));
     RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 20,     uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
@@ -619,6 +615,17 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, BundleAdjustment,         int, 0,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #endif
 
+    // Features matching approaches
+    RTABMAP_PARAM_STR(PyMatcher, Path,       "",           "Path to python script file (see available ones in rtabmap/corelib/src/pymatcher/*). See the header to see where the script should be copied.");
+	RTABMAP_PARAM(PyMatcher, Iterations,     int, 20,      "Sinkhorn iterations. Used by SuperGlue.");
+	RTABMAP_PARAM(PyMatcher, Threshold,      float, 0.2,   "Used by SuperGlue.");
+	RTABMAP_PARAM(PyMatcher, Cuda,           bool, true,   "Used by SuperGlue.");
+	RTABMAP_PARAM_STR(PyMatcher, Model,        "indoor",   "For SuperGlue, set only \"indoor\" or \"outdoor\". For OANet, set path to one of the pth file (e.g., \"OANet/model/gl3d/sift-4000/model_best.pth\").");
+
+	RTABMAP_PARAM(GMS, WithRotation,         bool, false,   "Take rotation transformation into account.");
+	RTABMAP_PARAM(GMS, WithScale,            bool, false,   "Take scale transformation into account.");
+	RTABMAP_PARAM(GMS, ThresholdFactor,      double, 6.0,   "The higher, the less matches.");
+
     // ICP registration parameters
     RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
     RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");

corelib/include/rtabmap/core/RegistrationVis.h

@@ -37,6 +37,10 @@ namespace rtabmap {
 
 class Feature2D;
 
+#ifdef RTABMAP_PYMATCHER
+class PyMatcher;
+#endif
+
 // Visual registration
 class RTABMAP_EXP RegistrationVis : public Registration
 {
@@ -50,6 +54,11 @@ public:
 	float getInlierDistance() const {return _inlierDistance;}
 	int getIterations() const {return _iterations;}
 	int getMinInliers() const {return _minInliers;}
+	int getNNType() const {return _nnType;}
+	float getNNDR() const {return _nndr;}
+	int getEstimationType() const {return _estimationType;}
+
+	const Feature2D * getDetector() const {return _detectorFrom;}
 
 protected:
 	virtual Transform computeTransformationImpl(
@@ -78,8 +87,11 @@ private:
 	int _flowIterations;
 	float _flowEps;
 	int _flowMaxLevel;
-	bool _bfCrossCheck;
 	float _nndr;
+	int _nnType;
+	bool _gmsWithRotation;
+	bool _gmsWithScale;
+	double _gmsThresholdFactor;
 	int _guessWinSize;
 	bool _guessMatchToProjection;
 	int _bundleAdjustment;
@@ -92,6 +104,10 @@ private:
 
 	Feature2D * _detectorFrom;
 	Feature2D * _detectorTo;
+
+#ifdef RTABMAP_PYMATCHER
+	PyMatcher * _pyMatcher;
+#endif
 };
 
 }

corelib/include/rtabmap/core/Rtabmap.h

@@ -156,16 +156,22 @@ public:
 	void rejectLastLoopClosure();
 	void deleteLastLocation();
 	void setOptimizedPoses(const std::map<int, Transform> & poses);
-	void get3DMap(std::map<int, Signature> & signatures,
-			std::map<int, Transform> & poses,
-			std::multimap<int, Link> & constraints,
-			bool optimized,
-			bool global) const;
+	Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid) const;
+	RTABMAP_DEPRECATED(
+		void get3DMap(std::map<int, Signature> & signatures,
+				std::map<int, Transform> & poses,
+				std::multimap<int, Link> & constraints,
+				bool optimized,
+				bool global) const, "Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true.");
 	void getGraph(std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
 			bool optimized,
 			bool global,
-			std::map<int, Signature> * signatures = 0);
+			std::map<int, Signature> * signatures = 0,
+			bool withImages = false,
+			bool withScan = false,
+			bool withUserData = false,
+			bool withGrid = false) const;
 	int detectMoreLoopClosures(
 			float clusterRadius = 0.5f,
 			float clusterAngle = M_PI/6.0f,
@@ -257,6 +263,7 @@ private:
 	float _proximityFilteringRadius;
 	bool _proximityRawPosesUsed;
 	float _proximityAngle;
+	bool _proximityOdomGuess;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	float _optimizationMaxError;
@@ -277,6 +284,7 @@ private:
 	double _lastProcessTime;
 	bool _someNodesHaveBeenTransferred;
 	float _distanceTravelled;
+	bool _optimizeFromGraphEndChanged;
 
 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.
@@ -306,6 +314,7 @@ private:
 	bool _currentSessionHasGPS;
 	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
 	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
+	std::map<int, Transform> _odomCacheAddLink; // used in localization mode when adding external link
 
 	// Planning stuff
 	int _pathStatus;

corelib/include/rtabmap/core/SensorData.h

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/GPS.h>
 #include <rtabmap/core/EnvSensor.h>
 #include <rtabmap/core/Landmark.h>
+#include <rtabmap/core/GlobalDescriptor.h>
 
 namespace rtabmap
 {
@@ -249,6 +250,11 @@ public:
 	const std::vector<cv::Point3f> & keypoints3D() const {return _keypoints3D;}
 	const cv::Mat & descriptors() const {return _descriptors;}
 
+	void addGlobalDescriptor(const GlobalDescriptor & descriptor) {_globalDescriptors.push_back(descriptor);}
+	void setGlobalDescriptors(const std::vector<GlobalDescriptor> & descriptors) {_globalDescriptors = descriptors;}
+	void clearGlobalDescriptors() {_globalDescriptors.clear();}
+	const std::vector<GlobalDescriptor> & globalDescriptors() const {return _globalDescriptors;}
+
 	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
 	const Transform & groundTruth() const {return groundTruth_;}
 
@@ -323,6 +329,9 @@ private:
 	std::vector<cv::Point3f> _keypoints3D;
 	cv::Mat _descriptors;
 
+	// global descriptors
+	std::vector<GlobalDescriptor> _globalDescriptors;
+
 	Transform groundTruth_;
 
 	Transform globalPose_;

corelib/include/rtabmap/core/Statistics.h

@@ -54,12 +54,14 @@ class RTABMAP_EXP Statistics
 {
 	RTABMAP_STATS(Loop, RejectedHypothesis,);
 	RTABMAP_STATS(Loop, Accepted_hypothesis_id,);
+	RTABMAP_STATS(Loop, Suppressed_hypothesis_id,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_id,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_value,);
 	RTABMAP_STATS(Loop, Vp_hypothesis,);
 	RTABMAP_STATS(Loop, Reactivate_id,);
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
 	RTABMAP_STATS(Loop, Hypothesis_reactivated,);
+	RTABMAP_STATS(Loop, Visual_words,);
 	RTABMAP_STATS(Loop, Visual_inliers,);
 	RTABMAP_STATS(Loop, Visual_matches,);
 	RTABMAP_STATS(Loop, Last_id,);
@@ -73,13 +75,31 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Landmark_detected_node_ref,);
 	RTABMAP_STATS(Loop, Visual_inliers_mean_dist,m);
 	RTABMAP_STATS(Loop, Visual_inliers_distribution,);
-	RTABMAP_STATS(Loop, Map_correction_norm, m);
-	RTABMAP_STATS(Loop, Map_correction_x, m);
-	RTABMAP_STATS(Loop, Map_correction_y, m);
-	RTABMAP_STATS(Loop, Map_correction_z, m);
-	RTABMAP_STATS(Loop, Map_correction_roll, deg);
-	RTABMAP_STATS(Loop, Map_correction_pitch, deg);
-	RTABMAP_STATS(Loop, Map_correction_yaw, deg);
+	//Odom correction
+	RTABMAP_STATS(Loop, Odom_correction_norm, m);
+	RTABMAP_STATS(Loop, Odom_correction_angle, deg);
+	RTABMAP_STATS(Loop, Odom_correction_x, m);
+	RTABMAP_STATS(Loop, Odom_correction_y, m);
+	RTABMAP_STATS(Loop, Odom_correction_z, m);
+	RTABMAP_STATS(Loop, Odom_correction_roll, deg);
+	RTABMAP_STATS(Loop, Odom_correction_pitch, deg);
+	RTABMAP_STATS(Loop, Odom_correction_yaw, deg);
+	// Map to Odom
+	RTABMAP_STATS(Loop, MapToOdom_norm, m);
+	RTABMAP_STATS(Loop, MapToOdom_angle, deg);
+	RTABMAP_STATS(Loop, MapToOdom_x, m);
+	RTABMAP_STATS(Loop, MapToOdom_y, m);
+	RTABMAP_STATS(Loop, MapToOdom_z, m);
+	RTABMAP_STATS(Loop, MapToOdom_roll, deg);
+	RTABMAP_STATS(Loop, MapToOdom_pitch, deg);
+	RTABMAP_STATS(Loop, MapToOdom_yaw, deg);
+	// Map to Base
+	RTABMAP_STATS(Loop, MapToBase_x, m);
+	RTABMAP_STATS(Loop, MapToBase_y, m);
+	RTABMAP_STATS(Loop, MapToBase_z, m);
+	RTABMAP_STATS(Loop, MapToBase_roll, deg);
+	RTABMAP_STATS(Loop, MapToBase_pitch, deg);
+	RTABMAP_STATS(Loop, MapToBase_yaw, deg);
 
 	RTABMAP_STATS(Proximity, Time_detections,);
 	RTABMAP_STATS(Proximity, Space_last_detection_id,);
@@ -159,6 +179,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Markers_detection, ms);
 
 	RTABMAP_STATS(Keypoint, Dictionary_size, words);
+	RTABMAP_STATS(Keypoint, Current_frame, words);
 	RTABMAP_STATS(Keypoint, Indexed_words, words);
 	RTABMAP_STATS(Keypoint, Index_memory_usage, KB);
 

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -43,7 +43,7 @@ public:
 			const cv::Size & imageSize2,
 			const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
 			const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
 
 	// if R and T are not null, left and right camera models should be valid to be rectified.
 	StereoCameraModel(
@@ -68,7 +68,7 @@ public:
 			double cx,
 			double cy,
 			double baseline,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			const cv::Size & imageSize = cv::Size(0,0));
 	//minimal to be saved
 	StereoCameraModel(
@@ -78,7 +78,7 @@ public:
 			double cx,
 			double cy,
 			double baseline,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			const cv::Size & imageSize = cv::Size(0,0));
 	virtual ~StereoCameraModel() {}
 

corelib/include/rtabmap/core/VWDictionary.h

@@ -55,6 +55,23 @@ public:
 		kNNUndef};
 	static const int ID_START;
 	static const int ID_INVALID;
+	static std::string nnStrategyName(NNStrategy strategy)
+	{
+		switch(strategy) {
+		case kNNFlannNaive:
+			return "FLANN NAIVE";
+		case kNNFlannKdTree:
+			return "FLANN KD-TREE";
+		case kNNFlannLSH:
+			return "FLANN LSH";
+		case kNNBruteForce:
+			return "BRUTE FORCE";
+		case kNNBruteForceGPU:
+			return "BRUTE FORCE GPU";
+		default:
+			return "Unknown";
+		}
+	}
 
 public:
 	VWDictionary(const ParametersMap & parameters = ParametersMap());

corelib/include/rtabmap/core/camera/CameraK4A.h

@@ -34,6 +34,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Version.h"
 #include "rtabmap/utilite/UTimer.h"
 
+#ifdef RTABMAP_K4A
+ #include <k4a/k4atypes.h>
+#endif
+
 namespace rtabmap
 {
 
@@ -67,6 +71,13 @@ private:
 private:
 
 #ifdef RTABMAP_K4A
+	k4a_device_t device_;
+	k4a_device_configuration_t config_;
+	k4a_calibration_t calibration_;
+	k4a_transformation_t transformation_;
+	k4a_capture_t capture_;
+	std::string serial_number_;
+
 	void* playbackHandle_;
 	void* transformationHandle_;
 	CameraModel model_;

corelib/include/rtabmap/core/camera/CameraMyntEye.h

@@ -61,6 +61,9 @@ public:
 	virtual bool odomProvided() const { return false; }
 
 	void publishInterIMU(bool enabled);
+	void setAutoExposure();
+	void setManualExposure(int gain=24, int brightness=120, int constrast=116);
+	void setIrControl(int value);
 
 protected:
 	/**
@@ -80,6 +83,11 @@ private:
 	std::string deviceName_;
 	bool apiRectification_;
 	bool apiDepth_;
+	bool autoExposure_;
+	int gain_;
+	int brightness_;
+	int contrast_;
+	int irControl_;
 	USemaphore dataReady_;
 	UMutex dataMutex_;
 	cv::Mat leftFrameBuffer_;
@@ -94,7 +102,7 @@ private:
 
 	double softTimeBegin_;
 	std::uint64_t hardTimeBegin_;
-	std::uint64_t unitHardTime;
+	std::uint64_t unitHardTime_;
 	std::vector<std::uint64_t> lastHardTimes_;
 	std::vector<std::uint64_t> acc_;
 #endif

corelib/include/rtabmap/core/camera/CameraOpenni.h

@@ -69,10 +69,17 @@ public:
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenni();
 #ifdef RTABMAP_OPENNI
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+	void image_cb (
+			const std::shared_ptr<openni_wrapper::Image>& rgb,
+			const std::shared_ptr<openni_wrapper::DepthImage>& depth,
+			float constant);
+#else
     void image_cb (
     		const boost::shared_ptr<openni_wrapper::Image>& rgb,
 			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
 			float constant);
+#endif
 #endif
 
     virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

corelib/include/rtabmap/core/odometry/OdometryF2M.h

@@ -79,7 +79,6 @@ private:
 	Signature * lastFrame_;
 	int lastFrameOldestNewId_;
 	std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
-	std::map<double, Transform> imus_;
 	bool initGravity_;
 
 	std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>

corelib/include/rtabmap/core/odometry/OdometryMono.h

@@ -29,11 +29,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYMONO_H_
 
 #include <rtabmap/core/Odometry.h>
+#include <rtabmap/core/Link.h>
 
 namespace rtabmap {
 
 class Memory;
-class Stereo;
+class Feature2D;
 
 class RTABMAP_EXP OdometryMono : public Odometry
 {
@@ -41,6 +42,7 @@ public:
 	OdometryMono(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
 	virtual ~OdometryMono();
 	virtual void reset(const Transform & initialPose);
+	virtual Odometry::Type getType() {return kTypeUndef;}
 
 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
@@ -56,7 +58,7 @@ private:
 	int pnpFlags_;
 	int pnpRefineIterations_;
 
-	Stereo * stereo_;
+	Feature2D * feature2D_;
 
 	Memory * memory_;
 	int localHistoryMaxSize_;
@@ -66,12 +68,14 @@ private:
 	float fundMatrixReprojError_;
 	float fundMatrixConfidence_;
 
-	cv::Mat refDepthOrRight_;
-	std::map<int, cv::Point2f> cornersMap_;
+	std::map<int, cv::Point2f> firstFrameGuessCorners_;
 	std::map<int, cv::Point3f> localMap_;
 	std::map<int, std::map<int, cv::Point3f> > keyFrameWords3D_;
 	std::map<int, Transform> keyFramePoses_;
+	std::multimap<int, Link> keyFrameLinks_;
+	std::map<int, CameraModel> keyFrameModels_;
 	float maxVariance_;
+	float keyFrameThr_;
 };
 
 }

corelib/include/rtabmap/core/stereo/stereoRectifyFisheye.h

@@ -37,10 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/calib3d/calib3d_c.h>
 
 #if CV_MAJOR_VERSION >= 4
-
-#if CV_MINOR_VERSION >= 3
 #include <opencv2/core/core_c.h>
-#endif
 
 // Opencv4 doesn't expose those functions below anymore, we should recopy all of them!
 int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian CV_DEFAULT(0))

corelib/include/rtabmap/core/util3d_features.h

@@ -78,14 +78,11 @@ std::map<int, cv::Point3f> RTABMAP_EXP generateWords3DMono(
 		const std::map<int, cv::KeyPoint> & previousKpts,
 		const CameraModel & cameraModel,
 		Transform & cameraTransform,
-		int pnpIterations = 100,
-		float pnpReprojError = 8.0f,
-		int pnpFlags = 0, // cv::SOLVEPNP_ITERATIVE
-		int pnpRefineIterations = 1,
-		float ransacParam1 = 3.0f,
-		float ransacParam2 = 0.99f,
+		float ransacReprojThreshold = 3.0f,
+		float ransacConfidence = 0.99f,
 		const std::map<int, cv::Point3f> & refGuess3D = std::map<int, cv::Point3f>(),
-		double * variance = 0);
+		double * variance = 0,
+		std::vector<int> * matchesOut = 0);
 
 std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,

corelib/src/CMakeLists.txt

@@ -117,6 +117,8 @@ SET(SRC_FILES
     clams/slam_calibrator.cpp
     
     opencv/ORBextractor.cc
+    opencv/solvepnp.cpp
+    opencv/five-point.cpp
 )
 
 IF(OpenCV_VERSION_MAJOR EQUAL 2)
@@ -125,10 +127,6 @@ SET(SRC_FILES
     opencv/Orb.cpp
 )
 ENDIF(OpenCV_VERSION_MAJOR EQUAL 2)
-SET(SRC_FILES
-    ${SRC_FILES}
-    opencv/solvepnp.cpp
-)
 
 # to get includes in visual studio
 IF(MSVC)
@@ -190,6 +188,24 @@ IF(TORCH_FOUND)
 	)
 ENDIF(TORCH_FOUND)
 
+IF(Python3_FOUND)
+	SET(LIBRARIES
+		${LIBRARIES}
+		Python3::Python
+	)
+	SET(SRC_FILES
+		${SRC_FILES}
+		pymatcher/PyMatcher.cpp
+	)
+	 SET(INCLUDE_DIRS
+	    ${TORCH_INCLUDE_DIRS}
+	    ${CMAKE_CURRENT_SOURCE_DIR}/pymatcher
+		${INCLUDE_DIRS}
+	)
+ENDIF(Python3_FOUND)
+
+
+
 IF(Freenect_FOUND)
 	IF(Freenect_DASH_INCLUDES)
 	   ADD_DEFINITIONS("-DFREENECT_DASH_INCLUDES")

corelib/src/CameraModel.cpp

@@ -37,7 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-CameraModel::CameraModel()
+CameraModel::CameraModel() :
+	localTransform_(0,0,1,0, -1,0,0,0, 0,-1,0,0)
 {
 
 }

corelib/src/DBDriver.cpp

@@ -683,11 +683,11 @@ void DBDriver::getNodeData(
 	if(uContains(_trashSignatures, signatureId))
 	{
 		const Signature * s = _trashSignatures.at(signatureId);
-		if(!s->sensorData().imageCompressed().empty() ||
-			!s->sensorData().laserScanCompressed().isEmpty() ||
-			!s->sensorData().userDataCompressed().empty() ||
-			s->sensorData().gridCellSize() != 0.0f ||
-			!s->isSaved())
+		if((!s->isSaved() ||
+			((!images || !s->sensorData().imageCompressed().empty()) &&
+			 (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
+			 (!userData || !s->sensorData().userDataCompressed().empty()) &&
+			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
 		{
 			data = (SensorData)s->sensorData();
 			found = true;
@@ -1140,10 +1140,10 @@ void DBDriver::addInfoAfterRun(
 	}
 }
 
-void DBDriver::addStatistics(const Statistics & statistics) const
+void DBDriver::addStatistics(const Statistics & statistics, bool saveWmState) const
 {
 	_dbSafeAccessMutex.lock();
-	addStatisticsQuery(statistics);
+	addStatisticsQuery(statistics, saveWmState);
 	_dbSafeAccessMutex.unlock();
 }
 

corelib/src/DBDriverSqlite3.cpp

@@ -842,22 +842,22 @@ long DBDriverSqlite3::getFeaturesMemoryUsedQuery() const
 		std::string query;
 		if(uStrNumCmp(_version, "0.13.0") >= 0)
 		{
-			query = "SELECT sum(length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
+			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
 					 "FROM Feature";
 		}
 		else if(uStrNumCmp(_version, "0.12.0") >= 0)
 		{
-			query = "SELECT sum(length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
+			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(octave) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
 					 "FROM Map_Node_Word";
 		}
 		else if(uStrNumCmp(_version, "0.11.2") >= 0)
 		{
-			query = "SELECT sum(length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
+			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(depth_x) + length(depth_y) + length(depth_z) + length(descriptor_size) + length(descriptor)) "
 					 "FROM Map_Node_Word";
 		}
 		else
 		{
-			query = "SELECT sum(length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(depth_x) + length(depth_y) + length(depth_z)) "
+			query = "SELECT sum(length(node_id) + length(word_id) + length(pos_x) + length(pos_y) + length(size) + length(dir) + length(response) + length(depth_x) + length(depth_y) + length(depth_z)) "
 					 "FROM Map_Node_Word";
 		}
 
@@ -3384,6 +3384,74 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 			ULOGGER_DEBUG("Time load %d calibrations=%fs", (int)nodes.size(), timer.ticks());
 		}
+
+		// load global descriptors
+		if(nodes.size() && uStrNumCmp(_version, "0.20.0") >= 0)
+		{
+			std::stringstream query3;
+			query3 << "SELECT type, info, data "
+					 "FROM GlobalDescriptor "
+					 "WHERE node_id = ? ";
+
+			rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+			{
+				// bind id
+				rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+				std::vector<GlobalDescriptor> globalDescriptors;
+
+				rc = sqlite3_step(ppStmt);
+				while(rc == SQLITE_ROW)
+				{
+					int index=0;
+					const void * data = 0;
+					int dataSize = 0;
+					int type = -1;
+					cv::Mat info;
+					cv::Mat dataMat;
+
+					type = sqlite3_column_int(ppStmt, index++);
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize && data)
+					{
+						info = rtabmap::uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
+					}
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize && data)
+					{
+						dataMat = rtabmap::uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
+					}
+
+					UASSERT(!dataMat.empty());
+					globalDescriptors.push_back(GlobalDescriptor(type, dataMat, info));
+
+					rc = sqlite3_step(ppStmt);
+				}
+				UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+				if(!globalDescriptors.empty())
+				{
+					(*iter)->sensorData().setGlobalDescriptors(globalDescriptors);
+					ULOGGER_DEBUG("Add %d global descriptors to node %d", (int)globalDescriptors.size(), (*iter)->id());
+				}
+
+				//reset
+				rc = sqlite3_reset(ppStmt);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			}
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			ULOGGER_DEBUG("Time load %d global descriptors=%fs", (int)nodes.size(), timer.ticks());
+		}
+
 		if(ids.size() != loaded)
 		{
 			UERROR("Some signatures not found in database");
@@ -3658,7 +3726,7 @@ void DBDriverSqlite3::loadLinksQuery(
 			query << "SELECT to_id, type, transform FROM Link ";
 		}
 		query << "WHERE from_id = " << signatureId;
-		if(typeIn != Link::kUndef)
+		if(typeIn < Link::kEnd)
 		{
 			if(uStrNumCmp(_version, "0.7.4") >= 0)
 			{
@@ -4235,6 +4303,27 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 		UDEBUG("Time=%fs", timer.ticks());
 
+		if(uStrNumCmp(_version, "0.20.0") >= 0)
+		{
+			// Global descriptor table
+			std::string query = queryStepGlobalDescriptor();
+			rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
+			{
+				for(size_t d=0; d<(*i)->sensorData().globalDescriptors().size(); ++d)
+				{
+					stepGlobalDescriptor(ppStmt, (*i)->id(), (*i)->sensorData().globalDescriptors()[d]);
+				}
+			}
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			UDEBUG("Time=%fs", timer.ticks());
+		}
+
 		if(uStrNumCmp(_version, "0.10.0") >= 0)
 		{
 			// Add SensorData
@@ -4519,7 +4608,7 @@ void DBDriverSqlite3::updateLaserScanQuery(
 	}
 }
 
-void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
+void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics, bool saveWmState) const
 {
 	UDEBUG("Ref ID = %d", statistics.refImageId());
 	if(_ppDb)
@@ -4570,7 +4659,7 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
 				cv::Mat compressedWmState;
 				if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
 				{
-					if(!statistics.wmState().empty())
+					if(saveWmState && !statistics.wmState().empty())
 					{
 						compressedWmState = compressData2(cv::Mat(1, statistics.wmState().size(), CV_32SC1, (void *)statistics.wmState().data()));
 						rc = sqlite3_bind_blob(ppStmt, index++, compressedWmState.data, compressedWmState.cols, SQLITE_STATIC);
@@ -6364,6 +6453,61 @@ void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 }
 
+std::string DBDriverSqlite3::queryStepGlobalDescriptor() const
+{
+	UASSERT(uStrNumCmp(_version, "0.20.0") >= 0);
+	return "INSERT INTO GlobalDescriptor(node_id, type, info, data) VALUES(?,?,?,?);";
+}
+void DBDriverSqlite3::stepGlobalDescriptor(sqlite3_stmt * ppStmt,
+		int nodeId,
+		const GlobalDescriptor & descriptor) const
+{
+	if(!ppStmt)
+	{
+		UFATAL("");
+	}
+	int rc = SQLITE_OK;
+	int index = 1;
+
+	//node_if
+	rc = sqlite3_bind_int(ppStmt, index++, nodeId);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	//type
+	rc = sqlite3_bind_int(ppStmt, index++, nodeId);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	//info
+	std::vector<unsigned char> infoBytes = rtabmap::compressData(descriptor.info());
+	if(infoBytes.empty())
+	{
+		rc = sqlite3_bind_null(ppStmt, index++);
+	}
+	else
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, infoBytes.data(), infoBytes.size(), SQLITE_STATIC);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	//data
+	std::vector<unsigned char> dataBytes = rtabmap::compressData(descriptor.data());
+	if(infoBytes.empty())
+	{
+		rc = sqlite3_bind_null(ppStmt, index++);
+	}
+	else
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, dataBytes.data(), dataBytes.size(), SQLITE_STATIC);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	rc=sqlite3_step(ppStmt);
+	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	rc = sqlite3_reset(ppStmt);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+}
+
 std::string DBDriverSqlite3::queryStepOccupancyGridUpdate() const
 {
 	UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);

corelib/src/EpipolarGeometry.cpp

@@ -292,8 +292,8 @@ cv::Mat EpipolarGeometry::findPFromE(const cv::Mat & E,
 cv::Mat EpipolarGeometry::findFFromWords(
 		const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
 		std::vector<uchar> & status,
-		double ransacParam1,
-		double ransacParam2)
+		double ransacReprojThreshold,
+		double ransacConfidence)
 {
 
 	status = std::vector<uchar>(pairs.size(), 0);
@@ -329,8 +329,8 @@ cv::Mat EpipolarGeometry::findFFromWords(
 				points2,
 				status,
 				cv::FM_RANSAC,
-				ransacParam1,
-				ransacParam2);
+				ransacReprojThreshold,
+				ransacConfidence);
 
 	ULOGGER_DEBUG("Find fundamental matrix (OpenCV) time = %fs", timer.ticks());
 

corelib/src/Features2d.cpp

@@ -44,7 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "opencv/ORBextractor.h"
 #endif
 
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 #include "superpoint_torch/SuperPoint.h"
 #endif
 
@@ -339,7 +339,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
 	{
 		UTimer timer;
-		ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
+		ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", (int)keypoints.size());
 		// Remove words under the new hessian threshold
 
 		// Sort words by hessian
@@ -365,10 +365,50 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 	}
 	else
 	{
+		ULOGGER_DEBUG("keeping all %d keypoints", (int)keypoints.size());
 		inliers.resize(keypoints.size(), true);
 	}
 }
 
+void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols)
+{
+	if(maxKeypoints <= 0 || (int)keypoints.size() <= maxKeypoints)
+	{
+		inliers.resize(keypoints.size(), true);
+		return;
+	}
+	UASSERT(gridCols>=1 && gridRows >=1);
+	UASSERT(imageSize.height>gridRows && imageSize.width>gridCols);
+	int rowSize = imageSize.height / gridRows;
+	int colSize = imageSize.width / gridCols;
+	int maxKeypointsPerCell = maxKeypoints / (gridRows * gridCols);
+	std::vector<std::vector<cv::KeyPoint> > keypointsPerCell(gridRows * gridCols);
+	std::vector<std::vector<int> > indexesPerCell(gridRows * gridCols);
+	for(size_t i=0; i<keypoints.size(); ++i)
+	{
+		int cellRow = int(keypoints[i].pt.y)/rowSize;
+		int cellCol = int(keypoints[i].pt.x)/colSize;
+		UASSERT(cellRow >=0 && cellRow < gridRows);
+		UASSERT(cellCol >=0 && cellCol < gridCols);
+
+		keypointsPerCell[cellRow*gridCols + cellCol].push_back(keypoints[i]);
+		indexesPerCell[cellRow*gridCols + cellCol].push_back(i);
+	}
+	inliers.resize(keypoints.size(), false);
+	for(size_t i=0; i<keypointsPerCell.size(); ++i)
+	{
+		std::vector<bool> inliersCell;
+		limitKeypoints(keypointsPerCell[i], inliersCell, maxKeypointsPerCell);
+		for(size_t j=0; j<inliersCell.size(); ++j)
+		{
+			if(inliersCell[j])
+			{
+				inliers.at(indexesPerCell[i][j]) = true;
+			}
+		}
+	}
+}
+
 cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
 {
 	return util2d::computeRoi(image, roiRatios);
@@ -414,10 +454,6 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpGridCols(), gridCols_);
 
 	UASSERT(gridRows_ >= 1 && gridCols_>=1);
-	if(maxFeatures_ > 0)
-	{
-		maxFeatures_ =	maxFeatures_ / (gridRows_ * gridCols_);
-	}
 
 	// convert ROI from string to vector
 	ParametersMap::const_iterator iter;
@@ -472,6 +508,8 @@ Feature2D * Feature2D::create(const ParametersMap & parameters)
 }
 Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
 {
+
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
 #ifndef RTABMAP_NONFREE
 	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
 	{
@@ -494,6 +532,18 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 #endif
 #endif
 
+#else // >= 4.3.0-dev
+
+#ifndef RTABMAP_NONFREE
+	if(type == Feature2D::kFeatureSurf)
+	{
+		UWARN("SURF features cannot be used because OpenCV was not built with nonfree module. SIFT is used instead.");
+		type = Feature2D::kFeatureSift;
+	}
+#endif
+
+#endif // 4.3.0-dev
+
 #if CV_MAJOR_VERSION < 3
 	if(type == Feature2D::kFeatureKaze)
 	{
@@ -515,7 +565,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	}
 #endif
 
-#ifndef RTABMAP_SP_TORCH
+#ifndef RTABMAP_SUPERPOINT_TORCH
 	if(type == Feature2D::kFeatureSuperPointTorch)
 	{
 		UWARN("SupertPoint Torch feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
@@ -559,7 +609,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	case Feature2D::kFeatureOrbOctree:
 		feature2D = new ORBOctree(parameters);
 		break;
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	case Feature2D::kFeatureSuperPointTorch:
 		feature2D = new SuperPointTorch(parameters);
 		break;
@@ -639,6 +689,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 	// Get keypoints
 	int rowSize = globalRoi.height / gridRows_;
 	int colSize = globalRoi.width / gridCols_;
+	int maxFeatures =	maxFeatures_ / (gridRows_ * gridCols_);
 	for (int i = 0; i<gridRows_; ++i)
 	{
 		for (int j = 0; j<gridCols_; ++j)
@@ -646,7 +697,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 			cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
 			std::vector<cv::KeyPoint> sub_keypoints;
 			sub_keypoints = this->generateKeypointsImpl(image, roi, mask);
-			limitKeypoints(sub_keypoints, maxFeatures_);
+			limitKeypoints(sub_keypoints, maxFeatures);
 			if(roi.x || roi.y)
 			{
 				// Adjust keypoint position to raw image
@@ -659,7 +710,8 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 			keypoints.insert( keypoints.end(), sub_keypoints.begin(), sub_keypoints.end() );
 		}
 	}
-	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
+	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (grid=%dx%d, mask empty=%d)",
+			timer.ticks(), keypoints.size(), gridCols_, gridRows_,  mask.empty()?1:0);
 
 	if(keypoints.size() && _subPixWinSize > 0 && _subPixIterations > 0)
 	{
@@ -891,7 +943,8 @@ SIFT::SIFT(const ParametersMap & parameters) :
 	nOctaveLayers_(Parameters::defaultSIFTNOctaveLayers()),
 	contrastThreshold_(Parameters::defaultSIFTContrastThreshold()),
 	edgeThreshold_(Parameters::defaultSIFTEdgeThreshold()),
-	sigma_(Parameters::defaultSIFTSigma())
+	sigma_(Parameters::defaultSIFTSigma()),
+	rootSIFT_(Parameters::defaultSIFTRootSIFT())
 {
 	parseParameters(parameters);
 }
@@ -908,7 +961,9 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold_);
 	Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers_);
 	Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);
+	Parameters::parse(parameters, Parameters::kSIFTRootSIFT(), rootSIFT_);
 
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
 #ifdef RTABMAP_NONFREE
 #if CV_MAJOR_VERSION < 3
 	_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
@@ -918,13 +973,16 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
+#else
+	_sift = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
+#endif
 }
 
 std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
-#ifdef RTABMAP_NONFREE
+#if defined(RTABMAP_NONFREE) || CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 3)
 	cv::Mat imgRoi(image, roi);
 	cv::Mat maskRoi;
 	if(!mask.empty())
@@ -942,8 +1000,25 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
-#ifdef RTABMAP_NONFREE
+#if defined(RTABMAP_NONFREE) || CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 3)
 	_sift->compute(image, keypoints, descriptors);
+
+	if( rootSIFT_ && !descriptors.empty())
+	{
+		UDEBUG("Performing RootSIFT...");
+		// see http://www.pyimagesearch.com/2015/04/13/implementing-rootsift-in-python-and-opencv/
+		// apply the Hellinger kernel by first L1-normalizing and taking the
+		// square-root
+		for(int i=0; i<descriptors.rows; ++i)
+		{
+			// By taking the L1 norm, followed by the square-root, we have
+			// already L2 normalized the feature vector and further normalization
+			// is not needed.
+			descriptors.row(i) = descriptors.row(i) / cv::sum(descriptors.row(i))[0];
+			cv::sqrt(descriptors.row(i), descriptors.row(i));
+		}
+	}
+
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
@@ -1866,11 +1941,11 @@ cv::Mat ORBOctree::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv
 //SuperPointTorch
 //////////////////////////
 SuperPointTorch::SuperPointTorch(const ParametersMap & parameters) :
-		path_(Parameters::defaultSPTorchModelPath()),
-		threshold_(Parameters::defaultSPTorchThreshold()),
-		nms_(Parameters::defaultSPTorchNMS()),
-		minDistance_(Parameters::defaultSPTorchMinDistance()),
-		cuda_(Parameters::defaultSPTorchCuda())
+		path_(Parameters::defaultSuperPointModelPath()),
+		threshold_(Parameters::defaultSuperPointThreshold()),
+		nms_(Parameters::defaultSuperPointNMS()),
+		minDistance_(Parameters::defaultSuperPointNMSRadius()),
+		cuda_(Parameters::defaultSuperPointCuda())
 {
 	parseParameters(parameters);
 }
@@ -1884,14 +1959,16 @@ void SuperPointTorch::parseParameters(const ParametersMap & parameters)
 	Feature2D::parseParameters(parameters);
 
 	std::string previousPath = path_;
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	bool previousCuda = cuda_;
-	Parameters::parse(parameters, Parameters::kSPTorchModelPath(), path_);
-	Parameters::parse(parameters, Parameters::kSPTorchThreshold(), threshold_);
-	Parameters::parse(parameters, Parameters::kSPTorchNMS(), nms_);
-	Parameters::parse(parameters, Parameters::kSPTorchMinDistance(), minDistance_);
-	Parameters::parse(parameters, Parameters::kSPTorchCuda(), cuda_);
+#endif
+	Parameters::parse(parameters, Parameters::kSuperPointModelPath(), path_);
+	Parameters::parse(parameters, Parameters::kSuperPointThreshold(), threshold_);
+	Parameters::parse(parameters, Parameters::kSuperPointNMS(), nms_);
+	Parameters::parse(parameters, Parameters::kSuperPointNMSRadius(), minDistance_);
+	Parameters::parse(parameters, Parameters::kSuperPointCuda(), cuda_);
 
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	if(superPoint_.get() == 0 || path_.compare(previousPath) != 0 || previousCuda != cuda_)
 	{
 		superPoint_ = cv::Ptr<SPDetector>(new SPDetector(path_, threshold_, nms_, minDistance_, cuda_));
@@ -1909,10 +1986,10 @@ void SuperPointTorch::parseParameters(const ParametersMap & parameters)
 
 std::vector<cv::KeyPoint> SuperPointTorch::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	UASSERT_MSG(roi.x==0 && roi.y ==0, "Not supporting ROI");
-	return superPoint_->detect(image);
+	return superPoint_->detect(image, mask);
 #else
 	UWARN("RTAB-Map is not built with SuperPoint Torch support so SuperPoint Torch feature cannot be used!");
 	return std::vector<cv::KeyPoint>();
@@ -1921,7 +1998,7 @@ std::vector<cv::KeyPoint> SuperPointTorch::generateKeypointsImpl(const cv::Mat &
 
 cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	return superPoint_->compute(keypoints);
 #else

corelib/src/Graph.cpp

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/search/kdtree.h>
 #include <pcl/common/eigen.h>
 #include <pcl/common/common.h>
+#include <pcl/common/point_tests.h>
 #include <set>
 #include <queue>
 #include <fstream>

corelib/src/Memory.cpp

@@ -90,6 +90,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
 	_mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
 	_depthAsMask(Parameters::defaultMemDepthAsMask()),
+	_stereoFromMotion(Parameters::defaultMemStereoFromMotion()),
 	_imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
 	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
 	_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
@@ -552,6 +553,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
 	Parameters::parse(params, Parameters::kMemSTMSize(), _maxStMemSize);
 	Parameters::parse(params, Parameters::kMemDepthAsMask(), _depthAsMask);
+	Parameters::parse(params, Parameters::kMemStereoFromMotion(), _stereoFromMotion);
 	Parameters::parse(params, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
 	Parameters::parse(params, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
 	Parameters::parse(params, Parameters::kMemCompressionParallelized(), _compressionParallelized);
@@ -1080,27 +1082,32 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
 			{
 				for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 				{
-					merge = true;
 					Signature * sTo = this->_getSignature(iter->first);
-					UASSERT(sTo!=0);
-					sTo->removeLink(s->id());
-					if(iter->second.type() != Link::kNeighbor &&
-					   iter->second.type() != Link::kNeighborMerged &&
-					   iter->second.type() != Link::kUndef)
+					if(sTo->id()!=s->id()) // Not Prior/Gravity links...
 					{
-						// link to all neighbors
-						for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
+						UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str());
+						sTo->removeLink(s->id());
+						if(iter->second.type() != Link::kNeighbor &&
+						   iter->second.type() != Link::kNeighborMerged &&
+						   iter->second.type() != Link::kUndef)
 						{
-							if(!sTo->hasLink(jter->second.to()))
+							// link to all neighbors
+							for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
 							{
-								Link l = iter->second.inverse().merge(
-										jter->second,
-										iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
-								sTo->addLink(l);
-								Signature * sB = this->_getSignature(l.to());
-								UASSERT(sB!=0);
-								UASSERT(!sB->hasLink(l.to()));
-								sB->addLink(l.inverse());
+								if(!sTo->hasLink(jter->second.to()))
+								{
+									UDEBUG("Merging link %d->%d (type=%d) to link %d->%d (type %d)",
+											iter->second.from(), iter->second.to(), iter->second.type(),
+											jter->second.from(), jter->second.to(), jter->second.type());
+									Link l = iter->second.inverse().merge(
+											jter->second,
+											iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
+									sTo->addLink(l);
+									Signature * sB = this->_getSignature(l.to());
+									UASSERT(sB!=0);
+									UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d", sB->id(), l.to()).c_str());
+									sB->addLink(l.inverse());
+								}
 							}
 						}
 					}
@@ -1347,7 +1354,10 @@ std::map<int, int> Memory::getNeighborsId(
 		) const
 {
 	UASSERT(maxGraphDepth >= 0);
-	//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
+	//DEBUG("signatureId=%d maxGraphDepth=%d maxCheckedInDatabase=%d incrementMarginOnLoop=%d "
+	//		"ignoreLoopIds=%d ignoreIntermediateNodes=%d ignoreLocalSpaceLoopIds=%d",
+	//		signatureId, maxGraphDepth, maxCheckedInDatabase, incrementMarginOnLoop?1:0,
+	//		ignoreLoopIds?1:0, ignoreIntermediateNodes?1:0, ignoreLocalSpaceLoopIds?1:0);
 	if(dbAccessTime)
 	{
 		*dbAccessTime = 0;
@@ -1405,6 +1415,10 @@ std::map<int, int> Memory::getNeighborsId(
 
 					UTimer timer;
 					_dbDriver->loadLinks(*jter, tmpLinks, ignoreLoopIds?Link::kAllWithoutLandmarks:Link::kAllWithLandmarks);
+					if(tmpLinks.empty())
+					{
+						UWARN("No links loaded for %d?!", *jter);
+					}
 					if(!ignoreLoopIds)
 					{
 						for(std::multimap<int, Link>::iterator kter=tmpLinks.begin(); kter!=tmpLinks.end();)
@@ -1414,6 +1428,11 @@ std::map<int, int> Memory::getNeighborsId(
 								tmpLandmarks.insert(*kter);
 								tmpLinks.erase(kter++);
 							}
+							else if(kter->second.from() == kter->second.to())
+							{
+								// ignore self-referring links
+								tmpLinks.erase(kter++);
+							}
 							else
 							{
 								++kter;
@@ -1429,7 +1448,8 @@ std::map<int, int> Memory::getNeighborsId(
 				// links
 				for(std::multimap<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
 				{
-					if( !uContains(ids, iter->first) && ignoredIds.find(iter->first) == ignoredIds.end())
+					if(!uContains(ids, iter->first) &&
+					   ignoredIds.find(iter->first) == ignoredIds.end())
 					{
 						UASSERT(iter->second.type() != Link::kUndef);
 						if(iter->second.type() == Link::kNeighbor ||
@@ -1631,12 +1651,12 @@ double Memory::getDbSavingTime() const
 	return _dbDriver?_dbDriver->getEmptyTrashesTime():0;
 }
 
-std::set<int> Memory::getAllSignatureIds() const
+std::set<int> Memory::getAllSignatureIds(bool ignoreChildren) const
 {
 	std::set<int> ids;
 	if(_dbDriver)
 	{
-		_dbDriver->getAllNodeIds(ids);
+		_dbDriver->getAllNodeIds(ids, ignoreChildren);
 	}
 	for(std::map<int, Signature*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
 	{
@@ -2003,11 +2023,11 @@ int Memory::cleanup()
 	return signatureRemoved;
 }
 
-void Memory::saveStatistics(const Statistics & statistics)
+void Memory::saveStatistics(const Statistics & statistics, bool saveWmState)
 {
 	if(_dbDriver)
 	{
-		_dbDriver->addStatistics(statistics);
+		_dbDriver->addStatistics(statistics, saveWmState);
 	}
 }
 
@@ -2698,13 +2718,13 @@ Transform Memory::computeTransform(
 	   (_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().isEmpty()) ||
 	   (_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
 	{
-		fromS.sensorData() = getNodeData(fromS.id());
+		fromS.sensorData() = getNodeData(fromS.id(), true, true, true, true);
 	}
 	if(((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) && toS.sensorData().imageCompressed().empty()) ||
 	   (_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().isEmpty()) ||
 	   (_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
 	{
-		toS.sensorData() = getNodeData(toS.id());
+		toS.sensorData() = getNodeData(toS.id(), true, true, true, true);
 	}
 	// uncompress only what we need
 	cv::Mat imgBuf, depthBuf, userBuf;
@@ -3815,33 +3835,33 @@ cv::Mat Memory::getImageCompressed(int signatureId) const
 	return image;
 }
 
-SensorData Memory::getNodeData(int nodeId, bool uncompressedData) const
+SensorData Memory::getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const
 {
-	//UDEBUG("nodeId=%d", nodeId);
+	//UDEBUG("");
 	SensorData r;
-	Signature * s = this->_getSignature(nodeId);
-	if(s && !s->sensorData().imageCompressed().empty())
+	const Signature * s = this->getSignature(locationId);
+	if(s && (!s->isSaved() ||
+			((!images || !s->sensorData().imageCompressed().empty()) &&
+			 (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
+			 (!userData || !s->sensorData().userDataCompressed().empty()) &&
+			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
 	{
 		r = s->sensorData();
 	}
 	else if(_dbDriver)
 	{
 		// load from database
-		_dbDriver->getNodeData(nodeId, r);
-	}
-
-	if(uncompressedData)
-	{
-		r.uncompressData();
+		_dbDriver->getNodeData(locationId, r, images, scan, userData, occupancyGrid);
 	}
 
 	return r;
 }
 
-void Memory::getNodeWords(int nodeId,
+void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
 		std::multimap<int, cv::KeyPoint> & words,
 		std::multimap<int, cv::Point3f> & words3,
-		std::multimap<int, cv::Mat> & wordsDescriptors)
+		std::multimap<int, cv::Mat> & wordsDescriptors,
+		std::vector<GlobalDescriptor> & globalDescriptors) const
 {
 	//UDEBUG("nodeId=%d", nodeId);
 	Signature * s = this->_getSignature(nodeId);
@@ -3850,6 +3870,7 @@ void Memory::getNodeWords(int nodeId,
 		words = s->getWords();
 		words3 = s->getWords3();
 		wordsDescriptors = s->getWordsDescriptors();
+		globalDescriptors = s->sensorData().globalDescriptors();
 	}
 	else if(_dbDriver)
 	{
@@ -3864,6 +3885,7 @@ void Memory::getNodeWords(int nodeId,
 			words = signatures.front()->getWords();
 			words3 = signatures.front()->getWords3();
 			wordsDescriptors = signatures.front()->getWordsDescriptors();
+			globalDescriptors = signatures.front()->sensorData().globalDescriptors();
 			if(loadedFromTrash.size())
 			{
 				//put back
@@ -3879,7 +3901,7 @@ void Memory::getNodeWords(int nodeId,
 
 void Memory::getNodeCalibration(int nodeId,
 		std::vector<CameraModel> & models,
-		StereoCameraModel & stereoModel)
+		StereoCameraModel & stereoModel) const
 {
 	//UDEBUG("nodeId=%d", nodeId);
 	Signature * s = this->_getSignature(nodeId);
@@ -3895,28 +3917,6 @@ void Memory::getNodeCalibration(int nodeId,
 	}
 }
 
-SensorData Memory::getSignatureDataConst(int locationId,
-		bool images, bool scan, bool userData, bool occupancyGrid) const
-{
-	//UDEBUG("");
-	SensorData r;
-	const Signature * s = this->getSignature(locationId);
-	if(s && (!s->sensorData().imageCompressed().empty() ||
-			!s->sensorData().laserScanCompressed().isEmpty() ||
-			!s->sensorData().userDataCompressed().empty() ||
-			s->sensorData().gridCellSize() != 0.0f))
-	{
-		r = s->sensorData();
-	}
-	else if(_dbDriver)
-	{
-		// load from database
-		_dbDriver->getNodeData(locationId, r, images, scan, userData, occupancyGrid);
-	}
-
-	return r;
-}
-
 void Memory::generateGraph(const std::string & fileName, const std::set<int> & ids)
 {
 	if(!_dbDriver)
@@ -4514,10 +4514,26 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures())
 		{
 			UASSERT((int)keypoints.size() == descriptors.rows);
-			Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
+			int inliersCount = 0;
+			if(_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1)
+			{
+				Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(), decimatedData.imageRaw().size(), _feature2D->getGridRows(), _feature2D->getGridCols());
+				for(size_t i=0; i<inliers.size(); ++i)
+				{
+					if(inliers[i])
+					{
+						++inliersCount;
+					}
+				}
+			}
+			else
+			{
+				Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
+				inliersCount = _feature2D->getMaxFeatures();
+			}
 
-			descriptorsForQuantization = cv::Mat(_feature2D->getMaxFeatures(), descriptors.cols, descriptors.type());
-			quantizedToRawIndices.resize(_feature2D->getMaxFeatures());
+			descriptorsForQuantization = cv::Mat(inliersCount, descriptors.cols, descriptors.type());
+			quantizedToRawIndices.resize(inliersCount);
 			unsigned int oi=0;
 			UASSERT((int)inliers.size() == descriptors.rows);
 			for(int k=0; k < descriptors.rows; ++k)
@@ -4537,7 +4553,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					++oi;
 				}
 			}
-			UASSERT((int)oi == _feature2D->getMaxFeatures());
+			UASSERT_MSG((int)oi == inliersCount,
+					uFormat("oi=%d inliersCount=%d (maxFeatures=%d, grid=%dx%d)",
+							oi, inliersCount, _feature2D->getMaxFeatures(), _feature2D->getGridCols(), _feature2D->getGridRows()).c_str());
 		}
 
 		// Quantization to vocabulary
@@ -4577,6 +4595,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	std::multimap<int, cv::KeyPoint> words;
 	std::multimap<int, cv::Point3f> words3D;
 	std::multimap<int, cv::Mat> wordsDescriptors;
+	int words3DValid = 0;
 	if(wordIds.size() > 0)
 	{
 		UASSERT(wordIds.size() == keypoints.size());
@@ -4600,6 +4619,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			if(keypoints3D.size())
 			{
 				words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
+				if(util3d::isFinite(keypoints3D.at(i)))
+				{
+					++words3DValid;
+				}
 			}
 			if(_rawDescriptorsKept)
 			{
@@ -4703,16 +4726,17 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UDEBUG("time post-decimation = %fs", t);
 	}
 
-	bool triangulateWordsWithoutDepth = !_depthAsMask;
-	if(!pose.isNull() &&
+	if(_stereoFromMotion &&
+		!pose.isNull() &&
 		cameraModels.size() == 1 &&
 		words.size() &&
-		(words3D.size() == 0 || (triangulateWordsWithoutDepth && words.size() == words3D.size())) &&
+		(words3D.size() == 0 || (words.size() == words3D.size() && words3DValid!=(int)words3D.size())) &&
 		_registrationPipeline->isImageRequired() &&
 		_signatures.size() &&
 		_signatures.rbegin()->second->mapId() == _idMapCount) // same map
 	{
-		UDEBUG("Generate 3D words using odometry");
+		UDEBUG("Generate 3D words using odometry (%s=true and words3DValid=%d/%d)",
+				Parameters::kMemStereoFromMotion().c_str(), words3DValid, (int)words3D.size());
 		Signature * previousS = _signatures.rbegin()->second;
 		if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
 		{
@@ -4735,7 +4759,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			cpCurrent.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
 			cpCurrent.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
 
+			// The following is used only to re-estimate the correspondences, the returned transform is ignored
 			Transform tmpt;
+			RegistrationVis reg(parameters_);
 			if(_registrationPipeline->isScanRequired())
 			{
 				// If icp is used, remove it to just do visual registration
@@ -4746,10 +4772,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				tmpt = _registrationPipeline->computeTransformationMod(cpCurrent, cpPrevious, cameraTransform);
 			}
-
 			UDEBUG("t=%s", tmpt.prettyPrint().c_str());
 
-			// compute 3D words by epipolar geometry with the previous signature
+			// compute 3D words by epipolar geometry with the previous signature using odometry motion
 			std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
 					uMultimapToMapUnique(cpCurrent.getWords()),
 					uMultimapToMapUnique(cpPrevious.getWords()),
@@ -5060,6 +5085,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	s->sensorData().setGroundTruth(data.groundTruth());
 	s->sensorData().setGPS(data.gps());
 	s->sensorData().setEnvSensors(data.envSensors());
+	s->sensorData().setGlobalDescriptors(data.globalDescriptors());
 
 	t = timer.ticks();
 	if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);

corelib/src/Odometry.cpp

@@ -186,7 +186,6 @@ Odometry::~Odometry()
 	{
 		delete particleFilters_[i];
 	}
-	particleFilters_.clear();
 }
 
 void Odometry::reset(const Transform & initialPose)
@@ -199,6 +198,8 @@ void Odometry::reset(const Transform & initialPose)
 	previousStamp_ = 0;
 	distanceTravelled_ = 0;
 	framesProcessed_ = 0;
+	imuLastTransform_.setNull();
+	imus_.clear();
 	if(_force3DoF || particleFilters_.size())
 	{
 		float x,y,z, roll,pitch,yaw;
@@ -382,6 +383,21 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 
+	// cache imu data
+	if(!data.imu().empty())
+	{
+		if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
+		{
+			Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
+			// orientation includes roll and pitch but not yaw in local transform
+			imus_.insert(std::make_pair(data.stamp(), Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse()));
+			if(imus_.size() > 1000)
+			{
+				imus_.erase(imus_.begin());
+			}
+		}
+	}
+
 	// KITTI datasets start with stamp=0
 	double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
 	Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform();
@@ -423,10 +439,24 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 
+	Transform imuCurrentTransform;
 	if(!guessIn.isNull())
 	{
 		guess = guessIn;
 	}
+	else if(!data.imu().empty() && !imus_.empty())
+	{
+		// replace orientation guess with IMU (if available)
+		imuCurrentTransform = Transform::getTransform(imus_, data.stamp());
+		if(!imuCurrentTransform.isNull() && !imuLastTransform_.isNull())
+		{
+			Transform orientation = imuLastTransform_.inverse() * imuCurrentTransform;
+			guess = Transform(
+					orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
+					orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
+					orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
+		}
+	}
 
 	UTimer time;
 	Transform t;
@@ -688,6 +718,8 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 		++framesProcessed_;
 
+		imuLastTransform_ = imuCurrentTransform;
+
 		return _pose *= t; // update
 	}
 	else if(_resetCurrentCount > 0)

corelib/src/Optimizer.cpp

@@ -489,6 +489,13 @@ std::map<int, Transform> Optimizer::optimizeBA(
 						model.localTransform(),
 						-signatures.at(iter->first).sensorData().stereoCameraModel().baseline()*model.fx());
 			}
+			else if(signatures.at(iter->first).sensorData().cameraModels().size() > 1)
+			{
+				UERROR("Multi-cameras (%d) is not supported (id=%d).",
+						signatures.at(iter->first).sensorData().cameraModels().size(),
+						iter->first);
+				return std::map<int, Transform>();
+			}
 			else
 			{
 				UERROR("Missing calibration for node %d", iter->first);

corelib/src/Parameters.cpp

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "SimpleIni.h"
 #include <opencv2/core/version.hpp>
 #include <pcl/pcl_config.h>
+#include <opencv2/opencv_modules.hpp>
 #ifndef DISABLE_VTK
 #include <vtkVersion.h>
 #endif
@@ -166,7 +167,7 @@ bool Parameters::isFeatureParameter(const std::string & parameter)
 			group.compare("GFTT") == 0 ||
 			group.compare("BRISK") == 0 ||
 			group.compare("KAZE") == 0 ||
-			group.compare("SPTorch") == 0;
+			group.compare("SuperPoint") == 0;
 }
 
 rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, bool vis, bool icp)
@@ -184,16 +185,9 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo
 			group.compare("Optimizer") == 0 ||
 			group.compare("g2o") == 0 ||
 			group.compare("GTSAM") == 0 ||
-			(vis && group.compare("Vis") == 0) ||
+			(vis && (group.compare("Vis") == 0 || group.compare("PyMatcher") == 0 || group.compare("GMS") == 0)) ||
 			iter->first.compare(kRtabmapPublishRAMUsage())==0)
 		{
-			if(stereo)
-			{
-				if(iter->first.compare(Parameters::kVisEstimationType()) == 0)
-				{
-					iter->second = "1"; // 3D->2D (PNP)
-				}
-			}
 			odomParameters.insert(*iter);
 		}
 	}
@@ -238,6 +232,20 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.20.
+		removedParameters_.insert(std::make_pair("SuperGlue/Path",           std::make_pair(true, Parameters::kPyMatcherPath())));
+		removedParameters_.insert(std::make_pair("SuperGlue/Iterations",     std::make_pair(true, Parameters::kPyMatcherIterations())));
+		removedParameters_.insert(std::make_pair("SuperGlue/MatchThreshold", std::make_pair(true, Parameters::kPyMatcherThreshold())));
+		removedParameters_.insert(std::make_pair("SuperGlue/Cuda",           std::make_pair(true, Parameters::kPyMatcherCuda())));
+		removedParameters_.insert(std::make_pair("SuperGlue/Indoor",         std::make_pair(false, Parameters::kPyMatcherModel())));
+
+		removedParameters_.insert(std::make_pair("Vis/CorCrossCheck",   std::make_pair(false, Parameters::kVisCorNNType())));
+		removedParameters_.insert(std::make_pair("SPTorch/ModelPath",   std::make_pair(true,  Parameters::kSuperPointModelPath())));
+		removedParameters_.insert(std::make_pair("SPTorch/Threshold",   std::make_pair(true,  Parameters::kSuperPointThreshold())));
+		removedParameters_.insert(std::make_pair("SPTorch/NMS",         std::make_pair(true,  Parameters::kSuperPointNMS())));
+		removedParameters_.insert(std::make_pair("SPTorch/MinDistance", std::make_pair(true,  Parameters::kSuperPointNMSRadius())));
+		removedParameters_.insert(std::make_pair("SPTorch/Cuda",        std::make_pair(true,  Parameters::kSuperPointCuda())));
+
 		// 0.19.4
 		removedParameters_.insert(std::make_pair("RGBD/MaxLocalizationDistance", std::make_pair(true,  Parameters::kRGBDMaxLoopClosureDistance())));
 
@@ -595,6 +603,14 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 #endif
 				str = "OpenCV:";
 				std::cout << str << std::setw(spacing - str.size()) << CV_VERSION << std::endl;
+#if CV_MAJOR_VERSION >= 3
+				str = "With OpenCV xfeatures2d:";
+#ifdef HAVE_OPENCV_XFEATURES2D
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+#endif
 				str = "With OpenCV nonfree:";
 #ifdef RTABMAP_NONFREE
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
@@ -608,7 +624,13 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With SuperPoint Torch:";
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With Python3:";
+#ifdef RTABMAP_PYMATCHER
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
@@ -720,6 +742,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With MYNT EYE S:";
+#ifdef RTABMAP_MYNTEYE
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With libpointmatcher:";
 #ifdef RTABMAP_POINTMATCHER

corelib/src/RegistrationVis.cpp

@@ -44,8 +44,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMath.h>
 #include <opencv2/core/core_c.h>
 
+#if defined(HAVE_OPENCV_XFEATURES2D) && (CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1))
+#include <opencv2/xfeatures2d.hpp> // For GMS matcher
+#endif
+
 #include <rtflann/flann.hpp>
 
+
+#ifdef RTABMAP_PYMATCHER
+	#include <pymatcher/PyMatcher.h>
+#endif
+
 namespace rtabmap {
 
 RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration * child) :
@@ -66,7 +75,10 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_flowEps(Parameters::defaultVisCorFlowEps()),
 		_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
 		_nndr(Parameters::defaultVisCorNNDR()),
-		_bfCrossCheck(Parameters::defaultVisCorCrossCheck()),
+		_nnType(Parameters::defaultVisCorNNType()),
+		_gmsWithRotation(Parameters::defaultGMSWithRotation()),
+		_gmsWithScale(Parameters::defaultGMSWithScale()),
+		_gmsThresholdFactor(Parameters::defaultGMSThresholdFactor()),
 		_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
 		_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
 		_bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
@@ -75,6 +87,10 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_maxInliersMeanDistance(Parameters::defaultVisMeanInliersDistance()),
 		_detectorFrom(0),
 		_detectorTo(0)
+#ifdef RTABMAP_PYMATCHER
+		,
+		_pyMatcher(0)
+#endif
 {
 	_featureParameters = Parameters::getDefaultParameters();
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
@@ -114,7 +130,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
 	Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
 	Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
-	Parameters::parse(parameters, Parameters::kVisCorCrossCheck(), _bfCrossCheck);
+	Parameters::parse(parameters, Parameters::kVisCorNNType(), _nnType);
+	Parameters::parse(parameters, Parameters::kGMSWithRotation(), _gmsWithRotation);
+	Parameters::parse(parameters, Parameters::kGMSWithScale(), _gmsWithScale);
+	Parameters::parse(parameters, Parameters::kGMSThresholdFactor(), _gmsThresholdFactor);
 	Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
 	Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
 	Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
@@ -131,6 +150,48 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
 	UASSERT_MSG(_iterations > 0, uFormat("value=%d", _iterations).c_str());
 
+	if(_nnType == 6)
+	{
+		// verify that we have Python3 support
+#ifndef RTABMAP_PYMATCHER
+		UWARN("%s is set to 6 but RTAB-Map is not built with Python3 support, using default %d.",
+				Parameters::kVisCorNNType().c_str(), Parameters::defaultVisCorNNType());
+		_nnType = Parameters::defaultVisCorNNType();
+#else
+		int iterations = _pyMatcher?_pyMatcher->iterations():Parameters::defaultPyMatcherIterations();
+		float matchThr = _pyMatcher?_pyMatcher->matchThreshold():Parameters::defaultPyMatcherThreshold();
+		std::string path = _pyMatcher?_pyMatcher->path():Parameters::defaultPyMatcherPath();
+		bool cuda = _pyMatcher?_pyMatcher->cuda():Parameters::defaultPyMatcherCuda();
+		std::string model = _pyMatcher?_pyMatcher->model():Parameters::defaultPyMatcherModel();
+		Parameters::parse(parameters, Parameters::kPyMatcherIterations(), iterations);
+		Parameters::parse(parameters, Parameters::kPyMatcherThreshold(), matchThr);
+		Parameters::parse(parameters, Parameters::kPyMatcherPath(), path);
+		Parameters::parse(parameters, Parameters::kPyMatcherCuda(), cuda);
+		Parameters::parse(parameters, Parameters::kPyMatcherModel(), model);
+		if(path.empty())
+		{
+			UERROR("%s parameter should be set to use Python3 matching (%s=6), using default %d.",
+					Parameters::kPyMatcherPath().c_str(),
+					Parameters::kVisCorNNType().c_str(),
+					Parameters::defaultVisCorNNType());
+			_nnType = Parameters::defaultVisCorNNType();
+		}
+		else
+		{
+			delete _pyMatcher;
+			_pyMatcher = new PyMatcher(path, matchThr, iterations, cuda, model);
+		}
+#endif
+	}
+#if !defined(HAVE_OPENCV_XFEATURES2D) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION<4 || CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<1))
+	else if(_nnType == 7)
+	{
+		UWARN("%s is set to 7 but RTAB-Map is not built with OpenCV's xfeatures2d support (OpenCV >= 3.4.1 also required), using default %d.",
+				Parameters::kVisCorNNType().c_str(), Parameters::defaultVisCorNNType());
+		_nnType = Parameters::defaultVisCorNNType();
+	}
+#endif
+
 	// override feature parameters
 	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
@@ -143,7 +204,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 
 	if(uContains(parameters, Parameters::kVisCorNNType()))
 	{
-		uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kVisCorNNType())));
+		if(_nnType<VWDictionary::kNNUndef)
+		{
+			uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_nnType)));
+		}
 	}
 	if(uContains(parameters, Parameters::kVisCorNNDR()))
 	{
@@ -200,6 +264,9 @@ RegistrationVis::~RegistrationVis()
 {
 	delete _detectorFrom;
 	delete _detectorTo;
+#ifdef RTABMAP_PYMATCHER
+	delete _pyMatcher;
+#endif
 }
 
 Transform RegistrationVis::computeTransformationImpl(
@@ -222,7 +289,10 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%f", Parameters::kVisCorFlowEps().c_str(), _flowEps);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
 	UDEBUG("%s=%f", Parameters::kVisCorNNDR().c_str(), _nndr);
-	UDEBUG("%s=%d", Parameters::kVisCorCrossCheck().c_str(), _bfCrossCheck?1:0);
+	UDEBUG("%s=%d", Parameters::kVisCorNNType().c_str(), _nnType);
+	UDEBUG("%s=%d", Parameters::kVisCorGuessWinSize().c_str(), _guessWinSize);
+	UDEBUG("%s=%d", Parameters::kVisCorGuessMatchToProjection().c_str(), _guessMatchToProjection?1:0);
+	UDEBUG("Feature Detector = %d", (int)_detectorFrom->getType());
 	UDEBUG("guess=%s", guess.prettyPrint().c_str());
 
 	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors,  %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
@@ -709,7 +779,8 @@ Transform RegistrationVis::computeTransformationImpl(
 				// If guess is set, limit the search of matches using optical flow window size
 				bool guessSet = !guess.isIdentity() && !guess.isNull();
 				if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() &&
-						isCalibrated) // needed for projection
+						isCalibrated &&  // needed for projection
+						_estimationType != 2) // To make sure we match all features for 2D->2D
 				{
 					UDEBUG("");
 					UASSERT((int)kptsTo.size() == descriptorsTo.rows);
@@ -807,9 +878,8 @@ Transform RegistrationVis::computeTransformationImpl(
 									descriptorsIndices.resize(oi);
 									UASSERT(oi >=2);
 
-
-									cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, _bfCrossCheck);
-									if(_bfCrossCheck)
+									cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, _nnType == 5);
+									if(_nnType == 5) // bruteforce cross check
 									{
 										std::vector<cv::DMatch> matches;
 										matcher.match(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches);
@@ -818,7 +888,7 @@ Transform RegistrationVis::computeTransformationImpl(
 											matchedIndex = descriptorsIndices.at(matches.at(0).trainIdx);
 										}
 									}
-									else
+									else // bruteforce knn
 									{
 										std::vector<std::vector<cv::DMatch> > matches;
 										matcher.knnMatch(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
@@ -829,7 +899,6 @@ Transform RegistrationVis::computeTransformationImpl(
 											matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
 										}
 									}
-
 								}
 								else if(indices[i].size() == 1)
 								{
@@ -957,8 +1026,8 @@ Transform RegistrationVis::computeTransformationImpl(
 										bruteForceDescCopy += bruteForceTimer.ticks();
 										UASSERT(oi >=2);
 
-										cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, _bfCrossCheck);
-										if(_bfCrossCheck)
+										cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, _nnType==5);
+										if(_nnType==5) // bruteforce cross check
 										{
 											std::vector<cv::DMatch> matches;
 											matcher.match(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches);
@@ -967,7 +1036,7 @@ Transform RegistrationVis::computeTransformationImpl(
 												matchedIndexTo = descriptorsIndices.at(matches.at(0).trainIdx);
 											}
 										}
-										else
+										else // bruteforce knn
 										{
 											std::vector<std::vector<cv::DMatch> > matches;
 											matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
@@ -1068,7 +1137,11 @@ Transform RegistrationVis::computeTransformationImpl(
 					// match between all descriptors
 					std::list<int> fromWordIds;
 					std::list<int> toWordIds;
-					if(_bfCrossCheck)
+#ifdef RTABMAP_PYMATCHER
+					if(_nnType == 5 || (_nnType == 6 && _pyMatcher) || _nnType==7)
+#else
+					if(_nnType == 5 || _nnType == 7) // bruteforce cross check or GMS
+#endif
 					{
 						std::vector<int> fromWordIdsV(descriptorsFrom.rows);
 						for (int i = 0; i < descriptorsFrom.rows; ++i)
@@ -1083,10 +1156,66 @@ Transform RegistrationVis::computeTransformationImpl(
 						}
 						if(descriptorsTo.rows)
 						{
-							cv::BFMatcher matcher(descriptorsFrom.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, true);
 							std::vector<int> toWordIdsV(descriptorsTo.rows, 0);
 							std::vector<cv::DMatch> matches;
-							matcher.match(descriptorsTo, descriptorsFrom, matches);
+#ifdef RTABMAP_PYMATCHER
+							if(_nnType == 6 && _pyMatcher &&
+								descriptorsTo.cols == descriptorsFrom.cols &&
+								descriptorsTo.rows == (int)kptsTo.size() &&
+								descriptorsTo.type() == CV_32F &&
+								descriptorsFrom.type() == CV_32F &&
+								descriptorsFrom.rows == (int)kptsFrom.size() &&
+								imageSize.width > 0 && imageSize.height > 0)
+							{
+								UDEBUG("Python matching");
+								matches = _pyMatcher->match(descriptorsTo, descriptorsFrom, kptsTo, kptsFrom, imageSize);
+							}
+							else
+							{
+								if(_nnType == 6 && _pyMatcher)
+								{
+									UDEBUG("Invalid inputs for Python matching (desc type=%d, only float descriptors supported), doing bruteforce matching instead.", descriptorsFrom.type());
+								}
+#else
+							{
+#endif
+								bool doCrossCheck = true;
+#ifdef HAVE_OPENCV_XFEATURES2D
+#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1)
+								cv::Size imageSizeFrom;
+								if(_nnType == 7)
+								{
+									imageSizeFrom = imageFrom.size();
+									if(imageSizeFrom.height == 0 || imageSizeFrom.width == 0)
+									{
+										imageSizeFrom = fromSignature.sensorData().cameraModels().size() == 1?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModel().left().imageSize();
+									}
+									if(imageSize.height > 0 && imageSize.width > 0 &&
+									   imageSizeFrom.height > 0 && imageSizeFrom.width > 0)
+									{
+										doCrossCheck = false;
+									}
+									else
+									{
+										UDEBUG("Invalid inputs for GMS matching, image size should be set for both inputs, doing bruteforce matching instead.");
+									}
+								}
+#endif
+#endif
+
+								UDEBUG("BruteForce matching%s", _nnType!=7?" with crosscheck":" with GMS");
+								cv::BFMatcher matcher(descriptorsFrom.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, doCrossCheck);
+								matcher.match(descriptorsTo, descriptorsFrom, matches);
+
+#if defined(HAVE_OPENCV_XFEATURES2D) && (CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1))
+								if(!doCrossCheck)
+								{
+									std::vector<cv::DMatch> matchesGMS;
+									cv::xfeatures2d::matchGMS(imageSize, imageSizeFrom, kptsTo, kptsFrom, matches, matchesGMS, _gmsWithRotation, _gmsWithScale, _gmsThresholdFactor);
+									matches = matchesGMS;
+								}
+#endif
+							}
 							for(size_t i=0; i<matches.size(); ++i)
 							{
 								toWordIdsV[matches[i].queryIdx] = fromWordIdsV[matches[i].trainIdx];
@@ -1104,6 +1233,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					}
 					else
 					{
+						UDEBUG("VWDictionary knn matching");
 						VWDictionary dictionary(_featureParameters);
 						if(orignalWordsFromIds.empty())
 						{
@@ -1241,21 +1371,20 @@ Transform RegistrationVis::computeTransformationImpl(
 					// we only need the camera transform, send guess words3 for scale estimation
 					Transform cameraTransform;
 					double variance = 1.0f;
+					std::vector<int> matchesV;
 					std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
 							uMultimapToMapUnique(signatureA->getWords()),
 							uMultimapToMapUnique(signatureB->getWords()),
 							cameraModel,
 							cameraTransform,
-							_iterations,
 							_PnPReprojError,
-							_PnPFlags, // cv::SOLVEPNP_ITERATIVE
-							_PnPRefineIterations,
-							1.0f,
 							0.99f,
 							uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
-							&variance);
+							&variance,
+							&matchesV);
 					covariances[dir] *= variance;
 					inliers[dir] = uKeys(inliers3D);
+					matches[dir] = matchesV;
 
 					if(!cameraTransform.isNull())
 					{
@@ -1274,7 +1403,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							}
 							else
 							{
-								msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variance);
+								msg = uFormat("Variance is too high! (Max %s=%f, variance=%f)", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar, variance);
 								UINFO(msg.c_str());
 							}
 						}
@@ -1752,7 +1881,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	}
 	else if(toSignature.sensorData().isValid())
 	{
-		UWARN("Missing correspondences for registration (%d->%d). fromWords = %d fromImageEmpty=%d toWords = %d toImageEmpty=%d",
+		msg = uFormat("Missing correspondences for registration (%d->%d). fromWords = %d fromImageEmpty=%d toWords = %d toImageEmpty=%d",
 				fromSignature.id(), toSignature.id(),
 				(int)fromSignature.getWords().size(), fromSignature.sensorData().imageRaw().empty()?1:0,
 				(int)toSignature.getWords().size(), toSignature.sensorData().imageRaw().empty()?1:0);
@@ -1763,6 +1892,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	info.rejectedMsg = msg;
 	info.covariance = covariance;
 
+	UDEBUG("inliers=%d/%d", info.inliers, info.matches);
 	UDEBUG("transform=%s", transform.prettyPrint().c_str());
 	return transform;
 }

corelib/src/RtabmapThread.cpp

@@ -128,6 +128,7 @@ void RtabmapThread::close(bool databaseSaved, const std::string & ouputDatabaseP
 
 void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
 {
+	UDEBUG("optimized=%s, full=%s, graphOnly=%s", optimized?"true":"false", full?"true":"false", graphOnly?"true":"false");
 	if(_rtabmap)
 	{
 		std::map<int, Signature> signatures;
@@ -138,23 +139,15 @@ void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
 		std::map<int, std::string> labels;
 		std::map<int, std::vector<unsigned char> > userDatas;
 
-		if(graphOnly)
-		{
-			_rtabmap->getGraph(poses,
-					constraints,
-					optimized,
-					full,
-					&signatures);
-		}
-		else
-		{
-			_rtabmap->get3DMap(
-					signatures,
-					poses,
-					constraints,
-					optimized,
-					full);
-		}
+		_rtabmap->getGraph(poses,
+				constraints,
+				optimized,
+				full,
+				&signatures,
+				!graphOnly,
+				!graphOnly,
+				!graphOnly,
+				!graphOnly);
 
 		this->post(new RtabmapEvent3DMap(
 				signatures,
@@ -217,7 +210,6 @@ void RtabmapThread::mainLoop()
 		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
 		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
 		Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
-		UASSERT(_dataBufferMaxSize >= 0);
 		UASSERT(_rate >= 0.0f);
 		_rtabmap->init(parameters, str);
 		break;
@@ -225,7 +217,6 @@ void RtabmapThread::mainLoop()
 		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
 		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
 		Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
-		UASSERT(_dataBufferMaxSize >= 0);
 		UASSERT(_rate >= 0.0f);
 		_rtabmap->parseParameters(parameters);
 		break;

corelib/src/VWDictionary.cpp

@@ -279,47 +279,50 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 
 void VWDictionary::setNNStrategy(NNStrategy strategy)
 {
-	if(strategy!=kNNUndef)
-	{
 #if CV_MAJOR_VERSION < 3
 #ifdef HAVE_OPENCV_GPU
-		if(strategy == kNNBruteForceGPU && !cv::gpu::getCudaEnabledDeviceCount())
-		{
-			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
-			strategy = kNNBruteForce;
-		}
+	if(strategy == kNNBruteForceGPU && !cv::gpu::getCudaEnabledDeviceCount())
+	{
+		UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
+		strategy = kNNBruteForce;
+	}
 #else
-		if(strategy == kNNBruteForceGPU)
-		{
-			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but OpenCV is not built with GPU/cuda module! Doing \"kNNBruteForce\" instead.");
-			strategy = kNNBruteForce;
-		}
+	if(strategy == kNNBruteForceGPU)
+	{
+		UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but OpenCV is not built with GPU/cuda module! Doing \"kNNBruteForce\" instead.");
+		strategy = kNNBruteForce;
+	}
 #endif
 #else
 #ifdef HAVE_OPENCV_CUDAFEATURES2D
-		if(strategy == kNNBruteForceGPU && !cv::cuda::getCudaEnabledDeviceCount())
-		{
-			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
-			strategy = kNNBruteForce;
-		}
+	if(strategy == kNNBruteForceGPU && !cv::cuda::getCudaEnabledDeviceCount())
+	{
+		UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
+		strategy = kNNBruteForce;
+	}
 #else
-		if(strategy == kNNBruteForceGPU)
-		{
-			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but OpenCV cudafeatures2d module is not found! Doing \"kNNBruteForce\" instead.");
-			strategy = kNNBruteForce;
-		}
+	if(strategy == kNNBruteForceGPU)
+	{
+		UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but OpenCV cudafeatures2d module is not found! Doing \"kNNBruteForce\" instead.");
+		strategy = kNNBruteForce;
+	}
 #endif
 #endif
 
-		bool update = _strategy != strategy;
-		_strategy = strategy;
-		if(update)
-		{
-			_dataTree = cv::Mat();
-			_notIndexedWords = uKeysSet(_visualWords);
-			_removedIndexedWords.clear();
-			this->update();
-		}
+	if(strategy>=kNNUndef)
+	{
+		UERROR("Nearest neighobr strategy \"%d\" chosen but this strategy cannot be used with a dictionary! Doing \"kNNBruteForce\" instead.");
+		strategy = kNNBruteForce;
+	}
+
+	bool update = _strategy != strategy;
+	_strategy = strategy;
+	if(update)
+	{
+		_dataTree = cv::Mat();
+		_notIndexedWords = uKeysSet(_visualWords);
+		_removedIndexedWords.clear();
+		this->update();
 	}
 }
 

corelib/src/camera/CameraFreenect.cpp

@@ -473,8 +473,10 @@ SensorData CameraFreenect::captureImage(CameraInfo * info)
 				freenectDevice_->getAccelerometerValues(x,y,z);
 				if(x != 0.0 && y != 0.0 && z != 0.0)
 				{
-					// frame of imu on kinect is x->right, y->down, z->backward
-					data.setIMU(IMU(cv::Vec3d(0,0,0), cv::Mat(), cv::Vec3d(x, y, z), cv::Mat(), Transform(0,0,-1,0, -1,0,0,0, 0,-1,0,0)));
+					 Transform opticalTransform(0,-1,0,0, 0,0,-1,0, 1,0,0,0);
+					 Transform base = this->getLocalTransform()*opticalTransform;
+					// frame of imu on kinect is x->left, y->up, z->forward
+					data.setIMU(IMU(cv::Vec3d(0,0,0), cv::Mat(), cv::Vec3d(x, y, z), cv::Mat(), base*Transform(0,0,1,0, 1,0,0,0, 0,1,0,0)));
 				}
 			}
 		}

corelib/src/camera/CameraK4A.cpp

@@ -54,14 +54,13 @@ CameraK4A::CameraK4A(
 	const Transform & localTransform) :
 	Camera(imageRate, localTransform)
 #ifdef RTABMAP_K4A
-	,playbackHandle_(NULL),
+	,deviceId_(deviceId),
+	playbackHandle_(NULL),
 	transformationHandle_(NULL),
-	deviceId_(deviceId),
 	ir_(false),
 	previousStamp_(0.0)
 #endif
 {
-	UERROR("CameraK4A: Live camera stream is not yet supported, only recorded mkv files are.");
 }
 
 CameraK4A::CameraK4A(
@@ -70,7 +69,9 @@ CameraK4A::CameraK4A(
 	const Transform & localTransform) :
 	Camera(imageRate, localTransform)
 #ifdef RTABMAP_K4A
-	,playbackHandle_(NULL),
+	,
+	device_(NULL),
+	playbackHandle_(NULL),
 	transformationHandle_(NULL),
 	deviceId_(-1),
 	fileName_(fileName),
@@ -88,19 +89,37 @@ CameraK4A::~CameraK4A()
 void CameraK4A::close()
 {
 #ifdef RTABMAP_K4A
-	if (playbackHandle_ != NULL)
+	if (!fileName_.empty())
 	{
-		k4a_playback_close((k4a_playback_t)playbackHandle_);
+		if (playbackHandle_ != NULL)
+		{
+			k4a_playback_close((k4a_playback_t)playbackHandle_);
+			playbackHandle_ = NULL;
+		}
+
+		if (transformationHandle_ != NULL)
+		{
+			k4a_transformation_destroy((k4a_transformation_t)transformationHandle_);
+			transformationHandle_ = NULL;
+		}
 	}
-	if (transformationHandle_ != NULL)
+	else
 	{
-		k4a_transformation_destroy((k4a_transformation_t)transformationHandle_);
+		if (device_ != NULL)
+		{
+			k4a_device_stop_imu(device_);
+
+			if (transformation_ != NULL)
+			{
+				k4a_transformation_destroy(transformation_);
+				transformation_ = NULL;
+			}
+
+			k4a_device_stop_cameras(device_);
+			k4a_device_close(device_);
+			device_ = NULL;
+		}
 	}
-	/*
-	// Shut down the camera when finished with application logic
-	k4a_device_stop_cameras(device);
-	k4a_device_close(device);
-	*/
 #endif
 }
 
@@ -115,10 +134,10 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 {
 #ifdef RTABMAP_K4A
 	
-	close();
-
 	if (!fileName_.empty())
 	{
+		close();
+
 		if (k4a_playback_open(fileName_.c_str(), (k4a_playback_t*)&playbackHandle_) != K4A_RESULT_SUCCEEDED)
 		{
 			UERROR("Failed to open recording \"%s\"", fileName_.c_str());
@@ -161,6 +180,7 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 		}
 
 		k4a_record_configuration_t config;
+
 		if (k4a_playback_get_record_configuration((k4a_playback_t)playbackHandle_, &config))
 		{
 			UERROR("Failed to getting recording configuration");
@@ -170,19 +190,26 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 	}
 	else if (deviceId_ >= 0)
 	{
-		UERROR("CameraK4A: Live camera stream is not yet supported, only recorded mkv files are.");
-		return false;
+		device_ = NULL;
 
-		/*uint32_t count = k4a_device_get_installed_count();
-		if (count == 0)
+		config_ = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
+		config_.camera_fps = K4A_FRAMES_PER_SECOND_15;
+		config_.depth_mode = K4A_DEPTH_MODE_WFOV_2X2BINNED;
+		config_.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
+		config_.color_resolution = K4A_COLOR_RESOLUTION_720P;
+
+		int device_count = k4a_device_get_installed_count();
+
+		if (device_count == 0)
 		{
 			UERROR("No k4a devices attached!");
 			return false;
 		}
 
+		UINFO("CameraK4A found k4a device attached");
+
 		// Open the first plugged in Kinect device
-		k4a_device_t device = NULL;
-		if (K4A_FAILED(k4a_device_open(K4A_DEVICE_DEFAULT, &device)))
+		if (K4A_FAILED(k4a_device_open(K4A_DEVICE_DEFAULT, &device_)))
 		{
 			UERROR("Failed to open k4a device!");
 			return false;
@@ -190,29 +217,76 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 
 		// Get the size of the serial number
 		size_t serial_size = 0;
-		k4a_device_get_serialnum(device, NULL, &serial_size);
+		k4a_device_get_serialnum(device_, NULL, &serial_size);
 
 		// Allocate memory for the serial, then acquire it
 		char *serial = (char*)(malloc(serial_size));
-		k4a_device_get_serialnum(device, serial, &serial_size);
-		UINFO("Opened device: %s", serial);
+		k4a_device_get_serialnum(device_, serial, &serial_size);
+		serial_number_.assign(serial, serial_size);
 		free(serial);
 
-		// Configure a stream of 4096x3072 BRGA color data at 15 frames per second
-		k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
-		config.camera_fps = K4A_FRAMES_PER_SECOND_15;
-		config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
-		config.color_resolution = K4A_COLOR_RESOLUTION_3072P;
+		UINFO("Opened K4A device: %s", serial_number_.c_str());
 
 		// Start the camera with the given configuration
-		if (K4A_FAILED(k4a_device_start_cameras(device, &config)))
+		if (K4A_FAILED(k4a_device_start_cameras(device_, &config_)))
 		{
 			UERROR("Failed to start cameras!");
-			k4a_device_close(device);
+			k4a_device_close(device_);
 			return false;
-		}*/
+		}
+
+		UINFO("K4A camera started successfully");
+
+		if (K4A_FAILED(k4a_device_get_calibration(device_, config_.depth_mode, config_.color_resolution, &calibration_)))
+		{
+			UERROR("k4a_device_get_calibration() failed!");
+			k4a_device_close(device_);
+			return false;
+		}
+
+                if (ir_)
+                {
+                        model_ = CameraModel(
+                                calibration_.depth_camera_calibration.intrinsics.parameters.param.fx,
+                                calibration_.depth_camera_calibration.intrinsics.parameters.param.fy,
+                                calibration_.depth_camera_calibration.intrinsics.parameters.param.cx,
+                                calibration_.depth_camera_calibration.intrinsics.parameters.param.cy,
+                                this->getLocalTransform(),
+                                0,
+                                cv::Size(calibration_.depth_camera_calibration.resolution_width, calibration_.depth_camera_calibration.resolution_height));
+                }
+                else
+                {
+                        model_ = CameraModel(
+                                calibration_.color_camera_calibration.intrinsics.parameters.param.fx,
+                                calibration_.color_camera_calibration.intrinsics.parameters.param.fy,
+                                calibration_.color_camera_calibration.intrinsics.parameters.param.cx,
+                                calibration_.color_camera_calibration.intrinsics.parameters.param.cy,
+                                this->getLocalTransform(),
+                                0,
+                                cv::Size(calibration_.color_camera_calibration.resolution_width, calibration_.color_camera_calibration.resolution_height));
+                }
+
+		transformation_ = k4a_transformation_create(&calibration_);
+
+		if (K4A_FAILED(k4a_device_start_imu(device_)))
+		{
+			UERROR("Failed to start K4A IMU");
+			close();
+			return false;
+		}
+
+		UINFO("K4a IMU started successfully");
+
+		// Get an initial capture to put the camera in the right state
+		if (K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_capture(device_, &capture_, K4A_WAIT_INFINITE))
+		{
+			k4a_capture_release(capture_);
+			return true;
+		}
+
+		return false;
 	}
-	
 	return true;
 #else
 	UERROR("CameraK4A: RTAB-Map is not built with Kinect for Azure SDK support!");
@@ -228,7 +302,10 @@ bool CameraK4A::isCalibrated() const
 std::string CameraK4A::getSerial() const
 {
 #ifdef RTABMAP_K4A
-	return fileName_.empty()?"":fileName_;
+	if(device_ != NULL)
+		return(serial_number_);
+	else
+		return fileName_.empty()?"":fileName_;
 #else
 	return "";
 #endif
@@ -443,9 +520,132 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 	}
 	else
 	{
-		UERROR("CameraK4A: Live camera stream is not yet supported, only recorded mkv files are.");
-	}
+		k4a_image_t ir_image_;
+		k4a_image_t depth_image_;
+		k4a_image_t rgb_image_;
+		k4a_imu_sample_t imu_sample_;
 
+		if (K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_capture(device_, &capture_, K4A_WAIT_INFINITE))
+		{
+			cv::Mat bgrCV;
+			cv::Mat depthCV;
+			IMU imu;
+
+			if (ir_)
+			{
+				// Retrieve IR image from capture
+				ir_image_ = k4a_capture_get_ir_image(capture_);
+
+				if(ir_image_ != NULL)
+				{
+					// Convert IR image
+					cv::Mat bgrCV16(k4a_image_get_height_pixels(ir_image_),
+							k4a_image_get_width_pixels(ir_image_),
+							CV_16UC1,
+							(void*)k4a_image_get_buffer(ir_image_));
+
+					bgrCV16.convertTo(bgrCV, CV_8U);
+
+					// Release the image
+					k4a_image_release(ir_image_);
+				}
+			}
+			else
+			{
+				// Retrieve RGB image from capture
+				rgb_image_ = k4a_capture_get_color_image(capture_);
+
+				if(rgb_image_ != NULL)
+				{
+					// Convert RGB image
+					if (k4a_image_get_format(rgb_image_) == K4A_IMAGE_FORMAT_COLOR_MJPG)
+					{
+						bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(rgb_image_),
+									CV_8UC1,
+									(void*)k4a_image_get_buffer(rgb_image_)));
+					}
+					else
+					{
+						cv::Mat bgra(k4a_image_get_height_pixels(rgb_image_),
+							     k4a_image_get_width_pixels(rgb_image_),
+							     CV_8UC4,
+							     (void*)k4a_image_get_buffer(rgb_image_));
+
+						cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
+					}
+
+					// Release the image
+					k4a_image_release(rgb_image_);
+				}
+			}
+
+			// Retrieve depth image from capture
+			depth_image_ = k4a_capture_get_depth_image(capture_);
+
+			if (depth_image_ != NULL)
+			{
+				if (ir_)
+				{
+					depthCV = cv::Mat(k4a_image_get_height_pixels(depth_image_),
+							  k4a_image_get_width_pixels(depth_image_),
+							  CV_16UC1,
+							  (void*)k4a_image_get_buffer(depth_image_)).clone();
+				}
+				else
+				{
+					k4a_image_t transformedDepth = NULL;
+
+					if (k4a_image_create(k4a_image_get_format(depth_image_),
+							     bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
+					{
+						if(k4a_transformation_depth_image_to_color_camera(transformation_, depth_image_, transformedDepth) == K4A_RESULT_SUCCEEDED)
+						{
+							depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth),
+									  k4a_image_get_width_pixels(transformedDepth),
+									  CV_16UC1,
+									  (void*)k4a_image_get_buffer(transformedDepth)).clone();
+						}
+						else
+						{
+							UERROR("K4A failed to register depth image");
+						}
+
+						k4a_image_release(transformedDepth);
+					}
+					else
+					{
+						UERROR("K4A failed to allocate registered depth image");
+					}
+				}
+				k4a_image_release(depth_image_);
+		 	}
+			k4a_capture_release(capture_);
+
+			// Get IMU sample, clear buffer
+			if(K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_imu_sample(device_, &imu_sample_, 60))
+			{
+				imu = IMU(cv::Vec3d(-1 * imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, -1 * imu_sample_.gyro_sample.xyz.z),
+					  cv::Mat::eye(3, 3, CV_64FC1),
+					  cv::Vec3d(-1 * imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, -1 * imu_sample_.acc_sample.xyz.z),
+					  cv::Mat::eye(3, 3, CV_64FC1),
+					  Transform::getIdentity());
+
+				UINFO("IMU: %f %f %f %f %f %f", imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z,
+								imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, imu_sample_.acc_sample.xyz.z);
+			}
+			else
+			{
+				UERROR("IMU data NULL");
+			}
+
+			// Relay the data to rtabmap
+			if (!bgrCV.empty() && !depthCV.empty())
+			{
+				data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), UTimer::now());
+				data.setIMU(imu);
+			}
+		}
+	}
 #else
 	UERROR("CameraK4A: RTAB-Map is not built with Kinect for Azure SDK support!");
 #endif

corelib/src/camera/CameraMyntEye.cpp

@@ -45,13 +45,17 @@ CameraMyntEye::CameraMyntEye(const std::string & device, bool apiRectification,
 			deviceName_(device),
 			apiRectification_(apiRectification),
 			apiDepth_(apiDepth),
+			autoExposure_(true),
+			gain_(24),
+			brightness_(120),
+			contrast_(116),
 			dataReady_(0),
 			lastFramesStamp_(0.0),
 			stamp_(0),
 			publishInterIMU_(false),
 			softTimeBegin_(0.0),
 			hardTimeBegin_(0),
-			unitHardTime(std::numeric_limits<std::uint32_t>::max()*10)
+			unitHardTime_(std::numeric_limits<std::uint32_t>::max()*10)
 #endif
 {
 #ifdef RTABMAP_MYNTEYE
@@ -133,13 +137,13 @@ inline bool is_overflow(std::uint64_t now, std::uint64_t pre, std::uint64_t unit
 
 double CameraMyntEye::checkUpTimeStamp(std::uint64_t _hard_time, std::uint8_t stream) {
 	UASSERT(stream < (std::uint8_t)mynteye::Stream::LAST+1);
-	if (is_overflow(_hard_time, lastHardTimes_[stream], unitHardTime)) {
+	if (is_overflow(_hard_time, lastHardTimes_[stream], unitHardTime_)) {
 		acc_[stream]++;
 	}
 
 	lastHardTimes_[stream] = _hard_time;
 
-	return hardTimeToSoftTime(acc_[stream] * unitHardTime + _hard_time);
+	return hardTimeToSoftTime(acc_[stream] * unitHardTime_ + _hard_time);
 }
 #endif
 
@@ -150,6 +154,34 @@ void CameraMyntEye::publishInterIMU(bool enabled)
 #endif
 }
 
+void CameraMyntEye::setAutoExposure()
+{
+#ifdef RTABMAP_MYNTEYE
+	autoExposure_ = true;
+#endif
+}
+
+void CameraMyntEye::setManualExposure(int gain, int brightness, int constrast)
+{
+#ifdef RTABMAP_MYNTEYE
+	UASSERT(gain>=0 && gain<=48);
+	UASSERT(brightness>=0 && brightness<=240);
+	UASSERT(constrast>=0 && constrast<=254);
+	autoExposure_ = false;
+	gain_ = gain;
+	brightness_ = brightness;
+	contrast_ = constrast;
+#endif
+}
+
+void CameraMyntEye::setIrControl(int value)
+{
+#ifdef RTABMAP_MYNTEYE
+	UASSERT(value>=0 && value<=160);
+	irControl_ = value;
+#endif
+}
+
 bool CameraMyntEye::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 #ifdef RTABMAP_MYNTEYE
@@ -430,6 +462,15 @@ bool CameraMyntEye::init(const std::string & calibrationFolder, const std::strin
 
 		});
 
+		api_->SetOptionValue(mynteye::Option::EXPOSURE_MODE, autoExposure_?0:1);
+		if(!autoExposure_)
+		{
+			api_->SetOptionValue(mynteye::Option::GAIN, gain_);
+			api_->SetOptionValue(mynteye::Option::BRIGHTNESS, brightness_);
+			api_->SetOptionValue(mynteye::Option::CONTRAST, contrast_);
+		}
+		api_->SetOptionValue(mynteye::Option::IR_CONTROL, irControl_);
+
 		api_->Start(mynteye::Source::ALL);
 		uSleep(500); // To buffer some imus before sending images
 		return true;

corelib/src/camera/CameraOpenni.cpp

@@ -76,10 +76,17 @@ CameraOpenni::~CameraOpenni()
 #endif
 }
 #ifdef RTABMAP_OPENNI
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+void CameraOpenni::image_cb (
+		const std::shared_ptr<openni_wrapper::Image>& rgb,
+		const std::shared_ptr<openni_wrapper::DepthImage>& depth,
+		float constant)
+#else
 void CameraOpenni::image_cb (
 		const boost::shared_ptr<openni_wrapper::Image>& rgb,
 		const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
 		float constant)
+#endif
 {
 	UScopeMutex s(dataMutex_);
 
@@ -123,10 +130,17 @@ bool CameraOpenni::init(const std::string & calibrationFolder, const std::string
 			interface_ = new pcl::OpenNIGrabber(deviceId_);
 		}
 
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+		std::function<void (
+				const std::shared_ptr<openni_wrapper::Image>&,
+				const std::shared_ptr<openni_wrapper::DepthImage>&,
+				float)> f = std::bind (&CameraOpenni::image_cb, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+#else
 		boost::function<void (
 				const boost::shared_ptr<openni_wrapper::Image>&,
 				const boost::shared_ptr<openni_wrapper::DepthImage>&,
 				float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
+#endif
 		connection_ = interface_->registerCallback (f);
 
 		interface_->start ();

corelib/src/clams/frame_projector.cpp

@@ -33,6 +33,7 @@ RTAB-Map integration: Mathieu Labbe
 #include <rtabmap/core/util3d.h>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <pcl/common/point_tests.h>
 
 using namespace std;
 using namespace Eigen;

corelib/src/odometry/OdometryF2M.cpp

@@ -151,13 +151,6 @@ OdometryF2M::~OdometryF2M()
 {
 	delete map_;
 	delete lastFrame_;
-	scansBuffer_.clear();
-	bundleWordReferences_.clear();
-	bundlePoses_.clear();
-	bundleLinks_.clear();
-	bundleModels_.clear();
-	bundlePoseReferences_.clear();
-	imus_.clear();
 	delete sba_;
 	delete regPipeline_;
 	UDEBUG("");
@@ -181,7 +174,6 @@ void OdometryF2M::reset(const Transform & initialPose)
 		bundlePoseReferences_.clear();
 		bundleSeq_ = 0;
 		lastFrameOldestNewId_ = 0;
-		imus_.clear();
 	}
 	initGravity_ = false;
 }
@@ -206,30 +198,27 @@ Transform OdometryF2M::computeTransform(
 		info->type = 0;
 	}
 
+	Transform imuT;
 	if(sba_ && sba_->gravitySigma() > 0.0f && !data.imu().empty())
 	{
-		if(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0)
+		if(imus().empty())
 		{
 			UERROR("IMU received doesn't have orientation set, it is ignored. If you are using RTAB-Map standalone, enable IMU filtering in Preferences->Source panel. On ROS, use \"imu_filter_madgwick\" or \"imu_complementary_filter\" packages to compute the orientation.");
 		}
 		else
 		{
-			Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
-			// orientation includes roll and pitch but not yaw in local transform
-			imus_.insert(std::make_pair(data.stamp(), Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse()));
-			if(imus_.size() > 1000)
-			{
-				imus_.erase(imus_.begin());
-			}
-
+			imuT = Transform::getTransform(imus(), data.stamp());
 			if(this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f)
 			{
-				Eigen::Quaterniond imuQuat = imus_.rbegin()->second.getQuaterniond();
-				Transform previous = this->getPose();
-				Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
-				UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
-				initGravity_ = true;
-				this->reset(newFramePose);
+				if(!imuT.isNull())
+				{
+					Eigen::Quaterniond imuQuat = imuT.getQuaterniond();
+					Transform previous = this->getPose();
+					Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
+					UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
+					initGravity_ = true;
+					this->reset(newFramePose);
+				}
 			}
 		}
 
@@ -276,7 +265,6 @@ Transform OdometryF2M::computeTransform(
 			std::map<int, Transform> bundlePoses;
 			std::multimap<int, Link> bundleLinks;
 			std::map<int, CameraModel> bundleModels;
-			std::map<int, StereoCameraModel> bundleStereoModels;
 
 			for(int guessIteration=0;
 					guessIteration<(!guess.isNull()&&regPipeline_->isImageRequired()?2:1) && transform.isNull();
@@ -292,7 +280,6 @@ Transform OdometryF2M::computeTransform(
 				bundlePoses.clear();
 				bundleLinks.clear();
 				bundleModels.clear();
-				bundleStereoModels.clear();
 
 				float maxCorrespondenceDistance = 0.0f;
 				float pmOutlierRatio = 0.0f;
@@ -371,14 +358,9 @@ Transform OdometryF2M::computeTransform(
 							bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.covariance.inv())));
 							bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
 
-							Transform imuT;
-							if(!imus_.empty())
+							if(!imuT.isNull())
 							{
-								imuT = Transform::getTransform(imus_, lastFrame_->getStamp());
-								if(!imuT.isNull())
-								{
-									bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
-								}
+								bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
 							}
 
 							CameraModel model;
@@ -509,6 +491,17 @@ Transform OdometryF2M::computeTransform(
 										std::multimap<int, Link>::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false);
 										UASSERT(iter != bundleLinks.end());
 										iter->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
+
+										iter = graph::findLink(bundleLinks, lastFrame_->id(), lastFrame_->id(), false);
+										if(info && iter!=bundleLinks.end() && iter->second.type() == Link::kGravity)
+										{
+											float rollImu,pitchImu,yaw;
+											iter->second.transform().getEulerAngles(rollImu, pitchImu, yaw);
+											float roll,pitch;
+											transform.getEulerAngles(roll, pitch, yaw);
+											info->gravityRollError = fabs(rollImu - roll);
+											info->gravityPitchError = fabs(pitchImu - pitch);
+										}
 									}
 								}
 								UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
@@ -1247,7 +1240,7 @@ Transform OdometryF2M::computeTransform(
 						bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
 						bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
 
-						if(!imus_.empty())
+						if(!imuT.isNull())
 						{
 							bundleIMUOrientations_.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, newFramePose)));
 						}
@@ -1278,7 +1271,7 @@ Transform OdometryF2M::computeTransform(
 						Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
 						if(p2n && minComplexity>0.0f)
 						{
-							complexity = util3d::computeNormalsComplexity(*mapCloudNormals);
+							complexity = util3d::computeNormalsComplexity(*mapCloudNormals, Transform::getIdentity(), lastFrame_->sensorData().laserScanRaw().is2d());
 							if(complexity > minComplexity)
 							{
 								frameValid = true;

corelib/src/opencv/five-point.cpp

@@ -0,0 +1,645 @@
+/* This is a 5-point algorithm contributed to OpenCV by the author, Bo Li.
+   It implements the 5-point algorithm solver from Nister's paper:
+   Nister, An efficient solution to the five-point relative pose problem, PAMI, 2004.
+*/
+
+/*  Copyright (c) 2013, Bo Li (prclibo@gmail.com), ETH Zurich
+    All rights reserved.
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions are met:
+        * Redistributions of source code must retain the above copyright
+          notice, this list of conditions and the following disclaimer.
+        * Redistributions in binary form must reproduce the above copyright
+          notice, this list of conditions and the following disclaimer in the
+          documentation and/or other materials provided with the distribution.
+        * Neither the name of the copyright holder nor the
+          names of its contributors may be used to endorse or promote products
+          derived from this software without specific prior written permission.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+    ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "solvepnp.h"
+
+using namespace cv;
+
+namespace cv3
+{
+
+class EMEstimatorCallback : public PointSetRegistrator::Callback
+{
+public:
+    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
+    {
+        Mat q1 = _m1.getMat(), q2 = _m2.getMat();
+        Mat Q1 = q1.reshape(1, (int)q1.total());
+        Mat Q2 = q2.reshape(1, (int)q2.total());
+
+        int n = Q1.rows;
+        Mat Q(n, 9, CV_64F);
+        Q.col(0) = Q1.col(0).mul( Q2.col(0) );
+        Q.col(1) = Q1.col(1).mul( Q2.col(0) );
+        Q.col(2) = Q2.col(0) * 1.0;
+        Q.col(3) = Q1.col(0).mul( Q2.col(1) );
+        Q.col(4) = Q1.col(1).mul( Q2.col(1) );
+        Q.col(5) = Q2.col(1) * 1.0;
+        Q.col(6) = Q1.col(0) * 1.0;
+        Q.col(7) = Q1.col(1) * 1.0;
+        Q.col(8) = 1.0;
+
+        Mat U, W, Vt;
+        SVD::compute(Q, W, U, Vt, SVD::MODIFY_A | SVD::FULL_UV);
+
+        Mat EE = Mat(Vt.t()).colRange(5, 9) * 1.0;
+        Mat A(10, 20, CV_64F);
+        EE = EE.t();
+        getCoeffMat(EE.ptr<double>(), A.ptr<double>());
+        EE = EE.t();
+
+        A = A.colRange(0, 10).inv() * A.colRange(10, 20);
+
+        double b[3 * 13];
+        Mat B(3, 13, CV_64F, b);
+        for (int i = 0; i < 3; i++)
+        {
+            Mat arow1 = A.row(i * 2 + 4) * 1.0;
+            Mat arow2 = A.row(i * 2 + 5) * 1.0;
+            Mat row1(1, 13, CV_64F, Scalar(0.0));
+            Mat row2(1, 13, CV_64F, Scalar(0.0));
+
+            row1.colRange(1, 4) = arow1.colRange(0, 3) * 1.0;
+            row1.colRange(5, 8) = arow1.colRange(3, 6) * 1.0;
+            row1.colRange(9, 13) = arow1.colRange(6, 10) * 1.0;
+
+            row2.colRange(0, 3) = arow2.colRange(0, 3) * 1.0;
+            row2.colRange(4, 7) = arow2.colRange(3, 6) * 1.0;
+            row2.colRange(8, 12) = arow2.colRange(6, 10) * 1.0;
+
+            B.row(i) = row1 - row2;
+        }
+
+        double c[11];
+        Mat coeffs(1, 11, CV_64F, c);
+        c[10] = (b[0]*b[17]*b[34]+b[26]*b[4]*b[21]-b[26]*b[17]*b[8]-b[13]*b[4]*b[34]-b[0]*b[21]*b[30]+b[13]*b[30]*b[8]);
+        c[9] = (b[26]*b[4]*b[22]+b[14]*b[30]*b[8]+b[13]*b[31]*b[8]+b[1]*b[17]*b[34]-b[13]*b[5]*b[34]+b[26]*b[5]*b[21]-b[0]*b[21]*b[31]-b[26]*b[17]*b[9]-b[1]*b[21]*b[30]+b[27]*b[4]*b[21]+b[0]*b[17]*b[35]-b[0]*b[22]*b[30]+b[13]*b[30]*b[9]+b[0]*b[18]*b[34]-b[27]*b[17]*b[8]-b[14]*b[4]*b[34]-b[13]*b[4]*b[35]-b[26]*b[18]*b[8]);
+        c[8] = (b[14]*b[30]*b[9]+b[14]*b[31]*b[8]+b[13]*b[31]*b[9]-b[13]*b[4]*b[36]-b[13]*b[5]*b[35]+b[15]*b[30]*b[8]-b[13]*b[6]*b[34]+b[13]*b[30]*b[10]+b[13]*b[32]*b[8]-b[14]*b[4]*b[35]-b[14]*b[5]*b[34]+b[26]*b[4]*b[23]+b[26]*b[5]*b[22]+b[26]*b[6]*b[21]-b[26]*b[17]*b[10]-b[15]*b[4]*b[34]-b[26]*b[18]*b[9]-b[26]*b[19]*b[8]+b[27]*b[4]*b[22]+b[27]*b[5]*b[21]-b[27]*b[17]*b[9]-b[27]*b[18]*b[8]-b[1]*b[21]*b[31]-b[0]*b[23]*b[30]-b[0]*b[21]*b[32]+b[28]*b[4]*b[21]-b[28]*b[17]*b[8]+b[2]*b[17]*b[34]+b[0]*b[18]*b[35]-b[0]*b[22]*b[31]+b[0]*b[17]*b[36]+b[0]*b[19]*b[34]-b[1]*b[22]*b[30]+b[1]*b[18]*b[34]+b[1]*b[17]*b[35]-b[2]*b[21]*b[30]);
+        c[7] = (b[14]*b[30]*b[10]+b[14]*b[32]*b[8]-b[3]*b[21]*b[30]+b[3]*b[17]*b[34]+b[13]*b[32]*b[9]+b[13]*b[33]*b[8]-b[13]*b[4]*b[37]-b[13]*b[5]*b[36]+b[15]*b[30]*b[9]+b[15]*b[31]*b[8]-b[16]*b[4]*b[34]-b[13]*b[6]*b[35]-b[13]*b[7]*b[34]+b[13]*b[30]*b[11]+b[13]*b[31]*b[10]+b[14]*b[31]*b[9]-b[14]*b[4]*b[36]-b[14]*b[5]*b[35]-b[14]*b[6]*b[34]+b[16]*b[30]*b[8]-b[26]*b[20]*b[8]+b[26]*b[4]*b[24]+b[26]*b[5]*b[23]+b[26]*b[6]*b[22]+b[26]*b[7]*b[21]-b[26]*b[17]*b[11]-b[15]*b[4]*b[35]-b[15]*b[5]*b[34]-b[26]*b[18]*b[10]-b[26]*b[19]*b[9]+b[27]*b[4]*b[23]+b[27]*b[5]*b[22]+b[27]*b[6]*b[21]-b[27]*b[17]*b[10]-b[27]*b[18]*b[9]-b[27]*b[19]*b[8]+b[0]*b[17]*b[37]-b[0]*b[23]*b[31]-b[0]*b[24]*b[30]-b[0]*b[21]*b[33]-b[29]*b[17]*b[8]+b[28]*b[4]*b[22]+b[28]*b[5]*b[21]-b[28]*b[17]*b[9]-b[28]*b[18]*b[8]+b[29]*b[4]*b[21]+b[1]*b[19]*b[34]-b[2]*b[21]*b[31]+b[0]*b[20]*b[34]+b[0]*b[19]*b[35]+b[0]*b[18]*b[36]-b[0]*b[22]*b[32]-b[1]*b[23]*b[30]-b[1]*b[21]*b[32]+b[1]*b[18]*b[35]-b[1]*b[22]*b[31]-b[2]*b[22]*b[30]+b[2]*b[17]*b[35]+b[1]*b[17]*b[36]+b[2]*b[18]*b[34]);
+        c[6] = (-b[14]*b[6]*b[35]-b[14]*b[7]*b[34]-b[3]*b[22]*b[30]-b[3]*b[21]*b[31]+b[3]*b[17]*b[35]+b[3]*b[18]*b[34]+b[13]*b[32]*b[10]+b[13]*b[33]*b[9]-b[13]*b[4]*b[38]-b[13]*b[5]*b[37]-b[15]*b[6]*b[34]+b[15]*b[30]*b[10]+b[15]*b[32]*b[8]-b[16]*b[4]*b[35]-b[13]*b[6]*b[36]-b[13]*b[7]*b[35]+b[13]*b[31]*b[11]+b[13]*b[30]*b[12]+b[14]*b[32]*b[9]+b[14]*b[33]*b[8]-b[14]*b[4]*b[37]-b[14]*b[5]*b[36]+b[16]*b[30]*b[9]+b[16]*b[31]*b[8]-b[26]*b[20]*b[9]+b[26]*b[4]*b[25]+b[26]*b[5]*b[24]+b[26]*b[6]*b[23]+b[26]*b[7]*b[22]-b[26]*b[17]*b[12]+b[14]*b[30]*b[11]+b[14]*b[31]*b[10]+b[15]*b[31]*b[9]-b[15]*b[4]*b[36]-b[15]*b[5]*b[35]-b[26]*b[18]*b[11]-b[26]*b[19]*b[10]-b[27]*b[20]*b[8]+b[27]*b[4]*b[24]+b[27]*b[5]*b[23]+b[27]*b[6]*b[22]+b[27]*b[7]*b[21]-b[27]*b[17]*b[11]-b[27]*b[18]*b[10]-b[27]*b[19]*b[9]-b[16]*b[5]*b[34]-b[29]*b[17]*b[9]-b[29]*b[18]*b[8]+b[28]*b[4]*b[23]+b[28]*b[5]*b[22]+b[28]*b[6]*b[21]-b[28]*b[17]*b[10]-b[28]*b[18]*b[9]-b[28]*b[19]*b[8]+b[29]*b[4]*b[22]+b[29]*b[5]*b[21]-b[2]*b[23]*b[30]+b[2]*b[18]*b[35]-b[1]*b[22]*b[32]-b[2]*b[21]*b[32]+b[2]*b[19]*b[34]+b[0]*b[19]*b[36]-b[0]*b[22]*b[33]+b[0]*b[20]*b[35]-b[0]*b[23]*b[32]-b[0]*b[25]*b[30]+b[0]*b[17]*b[38]+b[0]*b[18]*b[37]-b[0]*b[24]*b[31]+b[1]*b[17]*b[37]-b[1]*b[23]*b[31]-b[1]*b[24]*b[30]-b[1]*b[21]*b[33]+b[1]*b[20]*b[34]+b[1]*b[19]*b[35]+b[1]*b[18]*b[36]+b[2]*b[17]*b[36]-b[2]*b[22]*b[31]);
+        c[5] = (-b[14]*b[6]*b[36]-b[14]*b[7]*b[35]+b[14]*b[31]*b[11]-b[3]*b[23]*b[30]-b[3]*b[21]*b[32]+b[3]*b[18]*b[35]-b[3]*b[22]*b[31]+b[3]*b[17]*b[36]+b[3]*b[19]*b[34]+b[13]*b[32]*b[11]+b[13]*b[33]*b[10]-b[13]*b[5]*b[38]-b[15]*b[6]*b[35]-b[15]*b[7]*b[34]+b[15]*b[30]*b[11]+b[15]*b[31]*b[10]+b[16]*b[31]*b[9]-b[13]*b[6]*b[37]-b[13]*b[7]*b[36]+b[13]*b[31]*b[12]+b[14]*b[32]*b[10]+b[14]*b[33]*b[9]-b[14]*b[4]*b[38]-b[14]*b[5]*b[37]-b[16]*b[6]*b[34]+b[16]*b[30]*b[10]+b[16]*b[32]*b[8]-b[26]*b[20]*b[10]+b[26]*b[5]*b[25]+b[26]*b[6]*b[24]+b[26]*b[7]*b[23]+b[14]*b[30]*b[12]+b[15]*b[32]*b[9]+b[15]*b[33]*b[8]-b[15]*b[4]*b[37]-b[15]*b[5]*b[36]+b[29]*b[5]*b[22]+b[29]*b[6]*b[21]-b[26]*b[18]*b[12]-b[26]*b[19]*b[11]-b[27]*b[20]*b[9]+b[27]*b[4]*b[25]+b[27]*b[5]*b[24]+b[27]*b[6]*b[23]+b[27]*b[7]*b[22]-b[27]*b[17]*b[12]-b[27]*b[18]*b[11]-b[27]*b[19]*b[10]-b[28]*b[20]*b[8]-b[16]*b[4]*b[36]-b[16]*b[5]*b[35]-b[29]*b[17]*b[10]-b[29]*b[18]*b[9]-b[29]*b[19]*b[8]+b[28]*b[4]*b[24]+b[28]*b[5]*b[23]+b[28]*b[6]*b[22]+b[28]*b[7]*b[21]-b[28]*b[17]*b[11]-b[28]*b[18]*b[10]-b[28]*b[19]*b[9]+b[29]*b[4]*b[23]-b[2]*b[22]*b[32]-b[2]*b[21]*b[33]-b[1]*b[24]*b[31]+b[0]*b[18]*b[38]-b[0]*b[24]*b[32]+b[0]*b[19]*b[37]+b[0]*b[20]*b[36]-b[0]*b[25]*b[31]-b[0]*b[23]*b[33]+b[1]*b[19]*b[36]-b[1]*b[22]*b[33]+b[1]*b[20]*b[35]+b[2]*b[19]*b[35]-b[2]*b[24]*b[30]-b[2]*b[23]*b[31]+b[2]*b[20]*b[34]+b[2]*b[17]*b[37]-b[1]*b[25]*b[30]+b[1]*b[18]*b[37]+b[1]*b[17]*b[38]-b[1]*b[23]*b[32]+b[2]*b[18]*b[36]);
+        c[4] = (-b[14]*b[6]*b[37]-b[14]*b[7]*b[36]+b[14]*b[31]*b[12]+b[3]*b[17]*b[37]-b[3]*b[23]*b[31]-b[3]*b[24]*b[30]-b[3]*b[21]*b[33]+b[3]*b[20]*b[34]+b[3]*b[19]*b[35]+b[3]*b[18]*b[36]-b[3]*b[22]*b[32]+b[13]*b[32]*b[12]+b[13]*b[33]*b[11]-b[15]*b[6]*b[36]-b[15]*b[7]*b[35]+b[15]*b[31]*b[11]+b[15]*b[30]*b[12]+b[16]*b[32]*b[9]+b[16]*b[33]*b[8]-b[13]*b[6]*b[38]-b[13]*b[7]*b[37]+b[14]*b[32]*b[11]+b[14]*b[33]*b[10]-b[14]*b[5]*b[38]-b[16]*b[6]*b[35]-b[16]*b[7]*b[34]+b[16]*b[30]*b[11]+b[16]*b[31]*b[10]-b[26]*b[19]*b[12]-b[26]*b[20]*b[11]+b[26]*b[6]*b[25]+b[26]*b[7]*b[24]+b[15]*b[32]*b[10]+b[15]*b[33]*b[9]-b[15]*b[4]*b[38]-b[15]*b[5]*b[37]+b[29]*b[5]*b[23]+b[29]*b[6]*b[22]+b[29]*b[7]*b[21]-b[27]*b[20]*b[10]+b[27]*b[5]*b[25]+b[27]*b[6]*b[24]+b[27]*b[7]*b[23]-b[27]*b[18]*b[12]-b[27]*b[19]*b[11]-b[28]*b[20]*b[9]-b[16]*b[4]*b[37]-b[16]*b[5]*b[36]+b[0]*b[19]*b[38]-b[0]*b[24]*b[33]+b[0]*b[20]*b[37]-b[29]*b[17]*b[11]-b[29]*b[18]*b[10]-b[29]*b[19]*b[9]+b[28]*b[4]*b[25]+b[28]*b[5]*b[24]+b[28]*b[6]*b[23]+b[28]*b[7]*b[22]-b[28]*b[17]*b[12]-b[28]*b[18]*b[11]-b[28]*b[19]*b[10]-b[29]*b[20]*b[8]+b[29]*b[4]*b[24]+b[2]*b[18]*b[37]-b[0]*b[25]*b[32]+b[1]*b[18]*b[38]-b[1]*b[24]*b[32]+b[1]*b[19]*b[37]+b[1]*b[20]*b[36]-b[1]*b[25]*b[31]+b[2]*b[17]*b[38]+b[2]*b[19]*b[36]-b[2]*b[24]*b[31]-b[2]*b[22]*b[33]-b[2]*b[23]*b[32]+b[2]*b[20]*b[35]-b[1]*b[23]*b[33]-b[2]*b[25]*b[30]);
+        c[3] = (-b[14]*b[6]*b[38]-b[14]*b[7]*b[37]+b[3]*b[19]*b[36]-b[3]*b[22]*b[33]+b[3]*b[20]*b[35]-b[3]*b[23]*b[32]-b[3]*b[25]*b[30]+b[3]*b[17]*b[38]+b[3]*b[18]*b[37]-b[3]*b[24]*b[31]-b[15]*b[6]*b[37]-b[15]*b[7]*b[36]+b[15]*b[31]*b[12]+b[16]*b[32]*b[10]+b[16]*b[33]*b[9]+b[13]*b[33]*b[12]-b[13]*b[7]*b[38]+b[14]*b[32]*b[12]+b[14]*b[33]*b[11]-b[16]*b[6]*b[36]-b[16]*b[7]*b[35]+b[16]*b[31]*b[11]+b[16]*b[30]*b[12]+b[15]*b[32]*b[11]+b[15]*b[33]*b[10]-b[15]*b[5]*b[38]+b[29]*b[5]*b[24]+b[29]*b[6]*b[23]-b[26]*b[20]*b[12]+b[26]*b[7]*b[25]-b[27]*b[19]*b[12]-b[27]*b[20]*b[11]+b[27]*b[6]*b[25]+b[27]*b[7]*b[24]-b[28]*b[20]*b[10]-b[16]*b[4]*b[38]-b[16]*b[5]*b[37]+b[29]*b[7]*b[22]-b[29]*b[17]*b[12]-b[29]*b[18]*b[11]-b[29]*b[19]*b[10]+b[28]*b[5]*b[25]+b[28]*b[6]*b[24]+b[28]*b[7]*b[23]-b[28]*b[18]*b[12]-b[28]*b[19]*b[11]-b[29]*b[20]*b[9]+b[29]*b[4]*b[25]-b[2]*b[24]*b[32]+b[0]*b[20]*b[38]-b[0]*b[25]*b[33]+b[1]*b[19]*b[38]-b[1]*b[24]*b[33]+b[1]*b[20]*b[37]-b[2]*b[25]*b[31]+b[2]*b[20]*b[36]-b[1]*b[25]*b[32]+b[2]*b[19]*b[37]+b[2]*b[18]*b[38]-b[2]*b[23]*b[33]);
+        c[2] = (b[3]*b[18]*b[38]-b[3]*b[24]*b[32]+b[3]*b[19]*b[37]+b[3]*b[20]*b[36]-b[3]*b[25]*b[31]-b[3]*b[23]*b[33]-b[15]*b[6]*b[38]-b[15]*b[7]*b[37]+b[16]*b[32]*b[11]+b[16]*b[33]*b[10]-b[16]*b[5]*b[38]-b[16]*b[6]*b[37]-b[16]*b[7]*b[36]+b[16]*b[31]*b[12]+b[14]*b[33]*b[12]-b[14]*b[7]*b[38]+b[15]*b[32]*b[12]+b[15]*b[33]*b[11]+b[29]*b[5]*b[25]+b[29]*b[6]*b[24]-b[27]*b[20]*b[12]+b[27]*b[7]*b[25]-b[28]*b[19]*b[12]-b[28]*b[20]*b[11]+b[29]*b[7]*b[23]-b[29]*b[18]*b[12]-b[29]*b[19]*b[11]+b[28]*b[6]*b[25]+b[28]*b[7]*b[24]-b[29]*b[20]*b[10]+b[2]*b[19]*b[38]-b[1]*b[25]*b[33]+b[2]*b[20]*b[37]-b[2]*b[24]*b[33]-b[2]*b[25]*b[32]+b[1]*b[20]*b[38]);
+        c[1] = (b[29]*b[7]*b[24]-b[29]*b[20]*b[11]+b[2]*b[20]*b[38]-b[2]*b[25]*b[33]-b[28]*b[20]*b[12]+b[28]*b[7]*b[25]-b[29]*b[19]*b[12]-b[3]*b[24]*b[33]+b[15]*b[33]*b[12]+b[3]*b[19]*b[38]-b[16]*b[6]*b[38]+b[3]*b[20]*b[37]+b[16]*b[32]*b[12]+b[29]*b[6]*b[25]-b[16]*b[7]*b[37]-b[3]*b[25]*b[32]-b[15]*b[7]*b[38]+b[16]*b[33]*b[11]);
+        c[0] = -b[29]*b[20]*b[12]+b[29]*b[7]*b[25]+b[16]*b[33]*b[12]-b[16]*b[7]*b[38]+b[3]*b[20]*b[38]-b[3]*b[25]*b[33];
+
+        std::vector<Complex<double> > roots;
+        solvePoly(coeffs, roots);
+
+        std::vector<double> xs, ys, zs;
+        int count = 0;
+
+        Mat ematrix(10*3, 3, CV_64F);
+        double* e = ematrix.ptr<double>();
+        for (size_t i = 0; i < roots.size(); i++)
+        {
+            if (fabs(roots[i].im) > 1e-10) continue;
+            double z1 = roots[i].re;
+            double z2 = z1 * z1;
+            double z3 = z2 * z1;
+            double z4 = z3 * z1;
+
+            double bz[3][3];
+            for (int j = 0; j < 3; j++)
+            {
+                const double * br = b + j * 13;
+                bz[j][0] = br[0] * z3 + br[1] * z2 + br[2] * z1 + br[3];
+                bz[j][1] = br[4] * z3 + br[5] * z2 + br[6] * z1 + br[7];
+                bz[j][2] = br[8] * z4 + br[9] * z3 + br[10] * z2 + br[11] * z1 + br[12];
+            }
+
+            Mat Bz(3, 3, CV_64F, bz);
+            cv::Mat xy1;
+            SVD::solveZ(Bz, xy1);
+
+            if (fabs(xy1.at<double>(2)) < 1e-10) continue;
+            xs.push_back(xy1.at<double>(0) / xy1.at<double>(2));
+            ys.push_back(xy1.at<double>(1) / xy1.at<double>(2));
+            zs.push_back(z1);
+
+            cv::Mat Evec = EE.col(0) * xs.back() + EE.col(1) * ys.back() + EE.col(2) * zs.back() + EE.col(3);
+            Evec /= norm(Evec);
+
+            memcpy(e + count * 9, Evec.ptr(), 9 * sizeof(double));
+            count++;
+        }
+
+        ematrix.rowRange(0, count*3).copyTo(_model);
+        return count;
+    }
+
+protected:
+    void getCoeffMat(double *e, double *A) const
+    {
+        double ep2[36], ep3[36];
+        for (int i = 0; i < 36; i++)
+        {
+            ep2[i] = e[i] * e[i];
+            ep3[i] = ep2[i] * e[i];
+        }
+
+        A[0]=e[33]*e[28]*e[32]-e[33]*e[31]*e[29]+e[30]*e[34]*e[29]-e[30]*e[28]*e[35]-e[27]*e[32]*e[34]+e[27]*e[31]*e[35];
+        A[146]=.5000000000*e[6]*ep2[8]-.5000000000*e[6]*ep2[5]+.5000000000*ep3[6]+.5000000000*e[6]*ep2[7]-.5000000000*e[6]*ep2[4]+e[0]*e[2]*e[8]+e[3]*e[4]*e[7]+e[3]*e[5]*e[8]+e[0]*e[1]*e[7]-.5000000000*e[6]*ep2[1]-.5000000000*e[6]*ep2[2]+.5000000000*ep2[0]*e[6]+.5000000000*ep2[3]*e[6];
+        A[1]=e[30]*e[34]*e[2]+e[33]*e[1]*e[32]-e[3]*e[28]*e[35]+e[0]*e[31]*e[35]+e[3]*e[34]*e[29]-e[30]*e[1]*e[35]+e[27]*e[31]*e[8]-e[27]*e[32]*e[7]-e[30]*e[28]*e[8]-e[33]*e[31]*e[2]-e[0]*e[32]*e[34]+e[6]*e[28]*e[32]-e[33]*e[4]*e[29]+e[33]*e[28]*e[5]+e[30]*e[7]*e[29]+e[27]*e[4]*e[35]-e[27]*e[5]*e[34]-e[6]*e[31]*e[29];
+        A[147]=e[9]*e[27]*e[15]+e[9]*e[29]*e[17]+e[9]*e[11]*e[35]+e[9]*e[28]*e[16]+e[9]*e[10]*e[34]+e[27]*e[11]*e[17]+e[27]*e[10]*e[16]+e[12]*e[30]*e[15]+e[12]*e[32]*e[17]+e[12]*e[14]*e[35]+e[12]*e[31]*e[16]+e[12]*e[13]*e[34]+e[30]*e[14]*e[17]+e[30]*e[13]*e[16]+e[15]*e[35]*e[17]+e[15]*e[34]*e[16]-1.*e[15]*e[28]*e[10]-1.*e[15]*e[31]*e[13]-1.*e[15]*e[32]*e[14]-1.*e[15]*e[29]*e[11]+.5000000000*ep2[9]*e[33]+.5000000000*e[33]*ep2[16]-.5000000000*e[33]*ep2[11]+.5000000000*e[33]*ep2[12]+1.500000000*e[33]*ep2[15]+.5000000000*e[33]*ep2[17]-.5000000000*e[33]*ep2[10]-.5000000000*e[33]*ep2[14]-.5000000000*e[33]*ep2[13];
+        A[2]=-e[33]*e[22]*e[29]-e[33]*e[31]*e[20]-e[27]*e[32]*e[25]+e[27]*e[22]*e[35]-e[27]*e[23]*e[34]+e[27]*e[31]*e[26]+e[33]*e[28]*e[23]-e[21]*e[28]*e[35]+e[30]*e[25]*e[29]+e[24]*e[28]*e[32]-e[24]*e[31]*e[29]+e[18]*e[31]*e[35]-e[30]*e[28]*e[26]-e[30]*e[19]*e[35]+e[21]*e[34]*e[29]+e[33]*e[19]*e[32]-e[18]*e[32]*e[34]+e[30]*e[34]*e[20];
+        A[144]=e[18]*e[2]*e[17]+e[3]*e[21]*e[15]+e[3]*e[12]*e[24]+e[3]*e[23]*e[17]+e[3]*e[14]*e[26]+e[3]*e[22]*e[16]+e[3]*e[13]*e[25]+3.*e[6]*e[24]*e[15]+e[6]*e[26]*e[17]+e[6]*e[25]*e[16]+e[0]*e[20]*e[17]+e[0]*e[11]*e[26]+e[0]*e[19]*e[16]+e[0]*e[10]*e[25]+e[15]*e[26]*e[8]-1.*e[15]*e[20]*e[2]-1.*e[15]*e[19]*e[1]-1.*e[15]*e[22]*e[4]+e[15]*e[25]*e[7]-1.*e[15]*e[23]*e[5]+e[12]*e[21]*e[6]+e[12]*e[22]*e[7]+e[12]*e[4]*e[25]+e[12]*e[23]*e[8]+e[12]*e[5]*e[26]-1.*e[24]*e[11]*e[2]-1.*e[24]*e[10]*e[1]-1.*e[24]*e[13]*e[4]+e[24]*e[16]*e[7]-1.*e[24]*e[14]*e[5]+e[24]*e[17]*e[8]+e[21]*e[13]*e[7]+e[21]*e[4]*e[16]+e[21]*e[14]*e[8]+e[21]*e[5]*e[17]-1.*e[6]*e[23]*e[14]-1.*e[6]*e[20]*e[11]-1.*e[6]*e[19]*e[10]-1.*e[6]*e[22]*e[13]+e[9]*e[18]*e[6]+e[9]*e[0]*e[24]+e[9]*e[19]*e[7]+e[9]*e[1]*e[25]+e[9]*e[20]*e[8]+e[9]*e[2]*e[26]+e[18]*e[0]*e[15]+e[18]*e[10]*e[7]+e[18]*e[1]*e[16]+e[18]*e[11]*e[8];
+        A[3]=e[33]*e[10]*e[32]+e[33]*e[28]*e[14]-e[33]*e[13]*e[29]-e[33]*e[31]*e[11]+e[9]*e[31]*e[35]-e[9]*e[32]*e[34]+e[27]*e[13]*e[35]-e[27]*e[32]*e[16]+e[27]*e[31]*e[17]-e[27]*e[14]*e[34]+e[12]*e[34]*e[29]-e[12]*e[28]*e[35]+e[30]*e[34]*e[11]+e[30]*e[16]*e[29]-e[30]*e[10]*e[35]-e[30]*e[28]*e[17]+e[15]*e[28]*e[32]-e[15]*e[31]*e[29];
+        A[145]=e[0]*e[27]*e[6]+e[0]*e[28]*e[7]+e[0]*e[1]*e[34]+e[0]*e[29]*e[8]+e[0]*e[2]*e[35]+e[6]*e[34]*e[7]-1.*e[6]*e[32]*e[5]+e[6]*e[30]*e[3]+e[6]*e[35]*e[8]-1.*e[6]*e[29]*e[2]-1.*e[6]*e[28]*e[1]-1.*e[6]*e[31]*e[4]+e[27]*e[1]*e[7]+e[27]*e[2]*e[8]+e[3]*e[31]*e[7]+e[3]*e[4]*e[34]+e[3]*e[32]*e[8]+e[3]*e[5]*e[35]+e[30]*e[4]*e[7]+e[30]*e[5]*e[8]+.5000000000*ep2[0]*e[33]+1.500000000*e[33]*ep2[6]-.5000000000*e[33]*ep2[4]-.5000000000*e[33]*ep2[5]-.5000000000*e[33]*ep2[1]+.5000000000*e[33]*ep2[7]+.5000000000*e[33]*ep2[3]-.5000000000*e[33]*ep2[2]+.5000000000*e[33]*ep2[8];
+        A[4]=-e[0]*e[23]*e[16]+e[9]*e[4]*e[26]+e[9]*e[22]*e[8]-e[9]*e[5]*e[25]-e[9]*e[23]*e[7]+e[18]*e[4]*e[17]+e[18]*e[13]*e[8]-e[18]*e[5]*e[16]-e[18]*e[14]*e[7]+e[3]*e[16]*e[20]+e[3]*e[25]*e[11]-e[3]*e[10]*e[26]-e[3]*e[19]*e[17]+e[12]*e[7]*e[20]+e[12]*e[25]*e[2]-e[12]*e[1]*e[26]-e[12]*e[19]*e[8]+e[21]*e[7]*e[11]+e[21]*e[16]*e[2]-e[21]*e[1]*e[17]-e[21]*e[10]*e[8]+e[6]*e[10]*e[23]+e[6]*e[19]*e[14]-e[6]*e[13]*e[20]-e[6]*e[22]*e[11]+e[15]*e[1]*e[23]+e[15]*e[19]*e[5]-e[15]*e[4]*e[20]-e[15]*e[22]*e[2]+e[24]*e[1]*e[14]+e[24]*e[10]*e[5]-e[24]*e[4]*e[11]-e[24]*e[13]*e[2]+e[0]*e[13]*e[26]+e[0]*e[22]*e[17]-e[0]*e[14]*e[25];
+        A[150]=e[18]*e[19]*e[25]+.5000000000*ep3[24]-.5000000000*e[24]*ep2[23]+e[18]*e[20]*e[26]+e[21]*e[22]*e[25]+e[21]*e[23]*e[26]-.5000000000*e[24]*ep2[19]+.5000000000*ep2[21]*e[24]+.5000000000*e[24]*ep2[26]-.5000000000*e[24]*ep2[20]+.5000000000*ep2[18]*e[24]-.5000000000*e[24]*ep2[22]+.5000000000*e[24]*ep2[25];
+        A[5]=-e[3]*e[1]*e[35]-e[0]*e[32]*e[7]+e[27]*e[4]*e[8]+e[33]*e[1]*e[5]-e[33]*e[4]*e[2]+e[0]*e[4]*e[35]+e[3]*e[34]*e[2]-e[30]*e[1]*e[8]+e[30]*e[7]*e[2]-e[6]*e[4]*e[29]+e[3]*e[7]*e[29]+e[6]*e[1]*e[32]-e[0]*e[5]*e[34]-e[3]*e[28]*e[8]+e[0]*e[31]*e[8]+e[6]*e[28]*e[5]-e[6]*e[31]*e[2]-e[27]*e[5]*e[7];
+        A[151]=e[33]*e[16]*e[7]-1.*e[33]*e[14]*e[5]+e[33]*e[17]*e[8]+e[30]*e[13]*e[7]+e[30]*e[4]*e[16]+e[30]*e[14]*e[8]+e[30]*e[5]*e[17]+e[6]*e[27]*e[9]-1.*e[6]*e[28]*e[10]-1.*e[6]*e[31]*e[13]-1.*e[6]*e[32]*e[14]-1.*e[6]*e[29]*e[11]+e[9]*e[28]*e[7]+e[9]*e[1]*e[34]+e[9]*e[29]*e[8]+e[9]*e[2]*e[35]+e[27]*e[10]*e[7]+e[27]*e[1]*e[16]+e[27]*e[11]*e[8]+e[27]*e[2]*e[17]+e[3]*e[30]*e[15]+e[3]*e[12]*e[33]+e[3]*e[32]*e[17]+e[3]*e[14]*e[35]+e[3]*e[31]*e[16]+e[3]*e[13]*e[34]+3.*e[6]*e[33]*e[15]+e[6]*e[35]*e[17]+e[6]*e[34]*e[16]+e[0]*e[27]*e[15]+e[0]*e[9]*e[33]+e[0]*e[29]*e[17]+e[0]*e[11]*e[35]+e[0]*e[28]*e[16]+e[0]*e[10]*e[34]+e[15]*e[34]*e[7]-1.*e[15]*e[32]*e[5]+e[15]*e[35]*e[8]-1.*e[15]*e[29]*e[2]-1.*e[15]*e[28]*e[1]-1.*e[15]*e[31]*e[4]+e[12]*e[30]*e[6]+e[12]*e[31]*e[7]+e[12]*e[4]*e[34]+e[12]*e[32]*e[8]+e[12]*e[5]*e[35]-1.*e[33]*e[11]*e[2]-1.*e[33]*e[10]*e[1]-1.*e[33]*e[13]*e[4];
+        A[6]=e[6]*e[1]*e[5]-e[6]*e[4]*e[2]+e[3]*e[7]*e[2]+e[0]*e[4]*e[8]-e[0]*e[5]*e[7]-e[3]*e[1]*e[8];
+        A[148]=.5000000000*ep3[15]+e[9]*e[10]*e[16]-.5000000000*e[15]*ep2[11]+e[9]*e[11]*e[17]+.5000000000*ep2[12]*e[15]+.5000000000*e[15]*ep2[16]+.5000000000*e[15]*ep2[17]-.5000000000*e[15]*ep2[13]+.5000000000*ep2[9]*e[15]+e[12]*e[14]*e[17]-.5000000000*e[15]*ep2[10]-.5000000000*e[15]*ep2[14]+e[12]*e[13]*e[16];
+        A[7]=e[15]*e[28]*e[14]-e[15]*e[13]*e[29]-e[15]*e[31]*e[11]+e[33]*e[10]*e[14]-e[33]*e[13]*e[11]+e[9]*e[13]*e[35]-e[9]*e[32]*e[16]+e[9]*e[31]*e[17]-e[9]*e[14]*e[34]+e[27]*e[13]*e[17]-e[27]*e[14]*e[16]+e[12]*e[34]*e[11]+e[12]*e[16]*e[29]-e[12]*e[10]*e[35]-e[12]*e[28]*e[17]+e[30]*e[16]*e[11]-e[30]*e[10]*e[17]+e[15]*e[10]*e[32];
+        A[149]=e[18]*e[27]*e[24]+e[18]*e[28]*e[25]+e[18]*e[19]*e[34]+e[18]*e[29]*e[26]+e[18]*e[20]*e[35]+e[27]*e[19]*e[25]+e[27]*e[20]*e[26]+e[21]*e[30]*e[24]+e[21]*e[31]*e[25]+e[21]*e[22]*e[34]+e[21]*e[32]*e[26]+e[21]*e[23]*e[35]+e[30]*e[22]*e[25]+e[30]*e[23]*e[26]+e[24]*e[34]*e[25]+e[24]*e[35]*e[26]-1.*e[24]*e[29]*e[20]-1.*e[24]*e[31]*e[22]-1.*e[24]*e[32]*e[23]-1.*e[24]*e[28]*e[19]+1.500000000*e[33]*ep2[24]+.5000000000*e[33]*ep2[25]+.5000000000*e[33]*ep2[26]-.5000000000*e[33]*ep2[23]-.5000000000*e[33]*ep2[19]-.5000000000*e[33]*ep2[20]-.5000000000*e[33]*ep2[22]+.5000000000*ep2[18]*e[33]+.5000000000*ep2[21]*e[33];
+        A[9]=e[21]*e[25]*e[29]-e[27]*e[23]*e[25]+e[24]*e[19]*e[32]-e[21]*e[28]*e[26]-e[21]*e[19]*e[35]+e[18]*e[31]*e[26]-e[30]*e[19]*e[26]-e[24]*e[31]*e[20]+e[24]*e[28]*e[23]+e[27]*e[22]*e[26]+e[30]*e[25]*e[20]-e[33]*e[22]*e[20]+e[33]*e[19]*e[23]+e[21]*e[34]*e[20]-e[18]*e[23]*e[34]-e[24]*e[22]*e[29]-e[18]*e[32]*e[25]+e[18]*e[22]*e[35];
+        A[155]=e[12]*e[14]*e[8]+e[12]*e[5]*e[17]+e[15]*e[16]*e[7]+e[15]*e[17]*e[8]+e[0]*e[11]*e[17]+e[0]*e[9]*e[15]+e[0]*e[10]*e[16]+e[3]*e[14]*e[17]+e[3]*e[13]*e[16]+e[9]*e[10]*e[7]+e[9]*e[1]*e[16]+e[9]*e[11]*e[8]+e[9]*e[2]*e[17]-1.*e[15]*e[11]*e[2]-1.*e[15]*e[10]*e[1]-1.*e[15]*e[13]*e[4]-1.*e[15]*e[14]*e[5]+e[12]*e[3]*e[15]+e[12]*e[13]*e[7]+e[12]*e[4]*e[16]+.5000000000*ep2[12]*e[6]+1.500000000*ep2[15]*e[6]+.5000000000*e[6]*ep2[17]+.5000000000*e[6]*ep2[16]+.5000000000*e[6]*ep2[9]-.5000000000*e[6]*ep2[11]-.5000000000*e[6]*ep2[10]-.5000000000*e[6]*ep2[14]-.5000000000*e[6]*ep2[13];
+        A[8]=-e[9]*e[14]*e[16]-e[12]*e[10]*e[17]+e[9]*e[13]*e[17]-e[15]*e[13]*e[11]+e[15]*e[10]*e[14]+e[12]*e[16]*e[11];
+        A[154]=e[21]*e[14]*e[17]+e[21]*e[13]*e[16]+e[15]*e[26]*e[17]+e[15]*e[25]*e[16]-1.*e[15]*e[23]*e[14]-1.*e[15]*e[20]*e[11]-1.*e[15]*e[19]*e[10]-1.*e[15]*e[22]*e[13]+e[9]*e[20]*e[17]+e[9]*e[11]*e[26]+e[9]*e[19]*e[16]+e[9]*e[10]*e[25]+.5000000000*ep2[12]*e[24]+1.500000000*e[24]*ep2[15]+.5000000000*e[24]*ep2[17]+.5000000000*e[24]*ep2[16]+.5000000000*ep2[9]*e[24]-.5000000000*e[24]*ep2[11]-.5000000000*e[24]*ep2[10]-.5000000000*e[24]*ep2[14]-.5000000000*e[24]*ep2[13]+e[18]*e[11]*e[17]+e[18]*e[9]*e[15]+e[18]*e[10]*e[16]+e[12]*e[21]*e[15]+e[12]*e[23]*e[17]+e[12]*e[14]*e[26]+e[12]*e[22]*e[16]+e[12]*e[13]*e[25];
+        A[11]=-e[9]*e[5]*e[34]+e[9]*e[31]*e[8]-e[9]*e[32]*e[7]+e[27]*e[4]*e[17]+e[27]*e[13]*e[8]-e[27]*e[5]*e[16]-e[27]*e[14]*e[7]+e[0]*e[13]*e[35]-e[0]*e[32]*e[16]+e[0]*e[31]*e[17]-e[0]*e[14]*e[34]+e[9]*e[4]*e[35]+e[6]*e[10]*e[32]+e[6]*e[28]*e[14]-e[6]*e[13]*e[29]-e[6]*e[31]*e[11]+e[15]*e[1]*e[32]+e[3]*e[34]*e[11]+e[3]*e[16]*e[29]-e[3]*e[10]*e[35]-e[3]*e[28]*e[17]-e[12]*e[1]*e[35]+e[12]*e[7]*e[29]+e[12]*e[34]*e[2]-e[12]*e[28]*e[8]+e[15]*e[28]*e[5]-e[15]*e[4]*e[29]-e[15]*e[31]*e[2]+e[33]*e[1]*e[14]+e[33]*e[10]*e[5]-e[33]*e[4]*e[11]-e[33]*e[13]*e[2]+e[30]*e[7]*e[11]+e[30]*e[16]*e[2]-e[30]*e[1]*e[17]-e[30]*e[10]*e[8];
+        A[153]=e[21]*e[31]*e[7]+e[21]*e[4]*e[34]+e[21]*e[32]*e[8]+e[21]*e[5]*e[35]+e[30]*e[22]*e[7]+e[30]*e[4]*e[25]+e[30]*e[23]*e[8]+e[30]*e[5]*e[26]+3.*e[24]*e[33]*e[6]+e[24]*e[34]*e[7]+e[24]*e[35]*e[8]+e[33]*e[25]*e[7]+e[33]*e[26]*e[8]+e[0]*e[27]*e[24]+e[0]*e[18]*e[33]+e[0]*e[28]*e[25]+e[0]*e[19]*e[34]+e[0]*e[29]*e[26]+e[0]*e[20]*e[35]+e[18]*e[27]*e[6]+e[18]*e[28]*e[7]+e[18]*e[1]*e[34]+e[18]*e[29]*e[8]+e[18]*e[2]*e[35]+e[27]*e[19]*e[7]+e[27]*e[1]*e[25]+e[27]*e[20]*e[8]+e[27]*e[2]*e[26]+e[3]*e[30]*e[24]+e[3]*e[21]*e[33]+e[3]*e[31]*e[25]+e[3]*e[22]*e[34]+e[3]*e[32]*e[26]+e[3]*e[23]*e[35]+e[6]*e[30]*e[21]-1.*e[6]*e[29]*e[20]+e[6]*e[35]*e[26]-1.*e[6]*e[31]*e[22]-1.*e[6]*e[32]*e[23]-1.*e[6]*e[28]*e[19]+e[6]*e[34]*e[25]-1.*e[24]*e[32]*e[5]-1.*e[24]*e[29]*e[2]-1.*e[24]*e[28]*e[1]-1.*e[24]*e[31]*e[4]-1.*e[33]*e[20]*e[2]-1.*e[33]*e[19]*e[1]-1.*e[33]*e[22]*e[4]-1.*e[33]*e[23]*e[5];
+        A[10]=e[21]*e[25]*e[20]-e[21]*e[19]*e[26]+e[18]*e[22]*e[26]-e[18]*e[23]*e[25]-e[24]*e[22]*e[20]+e[24]*e[19]*e[23];
+        A[152]=e[3]*e[4]*e[25]+e[3]*e[23]*e[8]+e[3]*e[5]*e[26]+e[21]*e[4]*e[7]+e[21]*e[5]*e[8]+e[6]*e[25]*e[7]+e[6]*e[26]*e[8]+e[0]*e[19]*e[7]+e[0]*e[1]*e[25]+e[0]*e[20]*e[8]+e[0]*e[2]*e[26]-1.*e[6]*e[20]*e[2]-1.*e[6]*e[19]*e[1]-1.*e[6]*e[22]*e[4]-1.*e[6]*e[23]*e[5]+e[18]*e[1]*e[7]+e[18]*e[0]*e[6]+e[18]*e[2]*e[8]+e[3]*e[21]*e[6]+e[3]*e[22]*e[7]-.5000000000*e[24]*ep2[4]+.5000000000*e[24]*ep2[0]+1.500000000*e[24]*ep2[6]-.5000000000*e[24]*ep2[5]-.5000000000*e[24]*ep2[1]+.5000000000*e[24]*ep2[7]+.5000000000*e[24]*ep2[3]-.5000000000*e[24]*ep2[2]+.5000000000*e[24]*ep2[8];
+        A[13]=e[6]*e[28]*e[23]-e[6]*e[22]*e[29]-e[6]*e[31]*e[20]-e[3]*e[19]*e[35]+e[3]*e[34]*e[20]+e[3]*e[25]*e[29]-e[21]*e[1]*e[35]+e[21]*e[7]*e[29]+e[21]*e[34]*e[2]+e[24]*e[1]*e[32]+e[24]*e[28]*e[5]-e[24]*e[4]*e[29]-e[24]*e[31]*e[2]+e[33]*e[1]*e[23]+e[33]*e[19]*e[5]-e[33]*e[4]*e[20]-e[33]*e[22]*e[2]-e[21]*e[28]*e[8]+e[30]*e[7]*e[20]+e[30]*e[25]*e[2]-e[30]*e[1]*e[26]+e[18]*e[4]*e[35]-e[18]*e[5]*e[34]+e[18]*e[31]*e[8]-e[18]*e[32]*e[7]+e[27]*e[4]*e[26]+e[27]*e[22]*e[8]-e[27]*e[5]*e[25]-e[27]*e[23]*e[7]-e[3]*e[28]*e[26]-e[0]*e[32]*e[25]+e[0]*e[22]*e[35]-e[0]*e[23]*e[34]+e[0]*e[31]*e[26]-e[30]*e[19]*e[8]+e[6]*e[19]*e[32];
+        A[159]=.5000000000*ep2[18]*e[6]+.5000000000*ep2[21]*e[6]+1.500000000*ep2[24]*e[6]+.5000000000*e[6]*ep2[26]-.5000000000*e[6]*ep2[23]-.5000000000*e[6]*ep2[19]-.5000000000*e[6]*ep2[20]-.5000000000*e[6]*ep2[22]+.5000000000*e[6]*ep2[25]+e[21]*e[3]*e[24]+e[18]*e[20]*e[8]+e[21]*e[4]*e[25]+e[18]*e[19]*e[7]+e[18]*e[1]*e[25]+e[21]*e[22]*e[7]+e[21]*e[23]*e[8]+e[18]*e[0]*e[24]+e[18]*e[2]*e[26]+e[21]*e[5]*e[26]+e[24]*e[26]*e[8]-1.*e[24]*e[20]*e[2]-1.*e[24]*e[19]*e[1]-1.*e[24]*e[22]*e[4]+e[24]*e[25]*e[7]-1.*e[24]*e[23]*e[5]+e[0]*e[19]*e[25]+e[0]*e[20]*e[26]+e[3]*e[22]*e[25]+e[3]*e[23]*e[26];
+        A[12]=e[18]*e[4]*e[8]+e[3]*e[7]*e[20]+e[3]*e[25]*e[2]-e[3]*e[1]*e[26]-e[18]*e[5]*e[7]+e[6]*e[1]*e[23]+e[6]*e[19]*e[5]-e[6]*e[4]*e[20]-e[6]*e[22]*e[2]+e[21]*e[7]*e[2]-e[21]*e[1]*e[8]+e[24]*e[1]*e[5]-e[24]*e[4]*e[2]-e[3]*e[19]*e[8]+e[0]*e[4]*e[26]+e[0]*e[22]*e[8]-e[0]*e[5]*e[25]-e[0]*e[23]*e[7];
+        A[158]=e[9]*e[1]*e[7]+e[9]*e[0]*e[6]+e[9]*e[2]*e[8]+e[3]*e[12]*e[6]+e[3]*e[13]*e[7]+e[3]*e[4]*e[16]+e[3]*e[14]*e[8]+e[3]*e[5]*e[17]+e[12]*e[4]*e[7]+e[12]*e[5]*e[8]+e[6]*e[16]*e[7]+e[6]*e[17]*e[8]-1.*e[6]*e[11]*e[2]-1.*e[6]*e[10]*e[1]-1.*e[6]*e[13]*e[4]-1.*e[6]*e[14]*e[5]+e[0]*e[10]*e[7]+e[0]*e[1]*e[16]+e[0]*e[11]*e[8]+e[0]*e[2]*e[17]+.5000000000*ep2[3]*e[15]+1.500000000*e[15]*ep2[6]+.5000000000*e[15]*ep2[7]+.5000000000*e[15]*ep2[8]+.5000000000*ep2[0]*e[15]-.5000000000*e[15]*ep2[4]-.5000000000*e[15]*ep2[5]-.5000000000*e[15]*ep2[1]-.5000000000*e[15]*ep2[2];
+        A[15]=-e[15]*e[13]*e[2]-e[6]*e[13]*e[11]-e[15]*e[4]*e[11]+e[12]*e[16]*e[2]-e[3]*e[10]*e[17]+e[3]*e[16]*e[11]+e[0]*e[13]*e[17]-e[0]*e[14]*e[16]+e[15]*e[1]*e[14]-e[12]*e[10]*e[8]+e[9]*e[4]*e[17]+e[9]*e[13]*e[8]-e[9]*e[5]*e[16]-e[9]*e[14]*e[7]+e[15]*e[10]*e[5]+e[12]*e[7]*e[11]+e[6]*e[10]*e[14]-e[12]*e[1]*e[17];
+        A[157]=e[12]*e[30]*e[24]+e[12]*e[21]*e[33]+e[12]*e[31]*e[25]+e[12]*e[22]*e[34]+e[12]*e[32]*e[26]+e[12]*e[23]*e[35]+e[9]*e[27]*e[24]+e[9]*e[18]*e[33]+e[9]*e[28]*e[25]+e[9]*e[19]*e[34]+e[9]*e[29]*e[26]+e[9]*e[20]*e[35]+e[21]*e[30]*e[15]+e[21]*e[32]*e[17]+e[21]*e[14]*e[35]+e[21]*e[31]*e[16]+e[21]*e[13]*e[34]+e[30]*e[23]*e[17]+e[30]*e[14]*e[26]+e[30]*e[22]*e[16]+e[30]*e[13]*e[25]+e[15]*e[27]*e[18]+3.*e[15]*e[33]*e[24]-1.*e[15]*e[29]*e[20]+e[15]*e[35]*e[26]-1.*e[15]*e[31]*e[22]-1.*e[15]*e[32]*e[23]-1.*e[15]*e[28]*e[19]+e[15]*e[34]*e[25]+e[18]*e[29]*e[17]+e[18]*e[11]*e[35]+e[18]*e[28]*e[16]+e[18]*e[10]*e[34]+e[27]*e[20]*e[17]+e[27]*e[11]*e[26]+e[27]*e[19]*e[16]+e[27]*e[10]*e[25]-1.*e[24]*e[28]*e[10]-1.*e[24]*e[31]*e[13]-1.*e[24]*e[32]*e[14]+e[24]*e[34]*e[16]+e[24]*e[35]*e[17]-1.*e[24]*e[29]*e[11]-1.*e[33]*e[23]*e[14]+e[33]*e[25]*e[16]+e[33]*e[26]*e[17]-1.*e[33]*e[20]*e[11]-1.*e[33]*e[19]*e[10]-1.*e[33]*e[22]*e[13];
+        A[14]=e[18]*e[13]*e[17]+e[9]*e[13]*e[26]+e[9]*e[22]*e[17]-e[9]*e[14]*e[25]-e[18]*e[14]*e[16]-e[15]*e[13]*e[20]-e[15]*e[22]*e[11]+e[12]*e[16]*e[20]+e[12]*e[25]*e[11]-e[12]*e[10]*e[26]-e[12]*e[19]*e[17]+e[21]*e[16]*e[11]-e[21]*e[10]*e[17]-e[9]*e[23]*e[16]+e[24]*e[10]*e[14]-e[24]*e[13]*e[11]+e[15]*e[10]*e[23]+e[15]*e[19]*e[14];
+        A[156]=e[21]*e[12]*e[24]+e[21]*e[23]*e[17]+e[21]*e[14]*e[26]+e[21]*e[22]*e[16]+e[21]*e[13]*e[25]+e[24]*e[26]*e[17]+e[24]*e[25]*e[16]+e[9]*e[19]*e[25]+e[9]*e[18]*e[24]+e[9]*e[20]*e[26]+e[12]*e[22]*e[25]+e[12]*e[23]*e[26]+e[18]*e[20]*e[17]+e[18]*e[11]*e[26]+e[18]*e[19]*e[16]+e[18]*e[10]*e[25]-1.*e[24]*e[23]*e[14]-1.*e[24]*e[20]*e[11]-1.*e[24]*e[19]*e[10]-1.*e[24]*e[22]*e[13]+.5000000000*ep2[21]*e[15]+1.500000000*ep2[24]*e[15]+.5000000000*e[15]*ep2[25]+.5000000000*e[15]*ep2[26]+.5000000000*e[15]*ep2[18]-.5000000000*e[15]*ep2[23]-.5000000000*e[15]*ep2[19]-.5000000000*e[15]*ep2[20]-.5000000000*e[15]*ep2[22];
+        A[18]=e[6]*e[1]*e[14]+e[15]*e[1]*e[5]-e[0]*e[5]*e[16]-e[0]*e[14]*e[7]+e[0]*e[13]*e[8]-e[15]*e[4]*e[2]+e[12]*e[7]*e[2]+e[6]*e[10]*e[5]+e[3]*e[7]*e[11]-e[6]*e[4]*e[11]+e[3]*e[16]*e[2]-e[6]*e[13]*e[2]-e[3]*e[1]*e[17]-e[9]*e[5]*e[7]-e[3]*e[10]*e[8]-e[12]*e[1]*e[8]+e[0]*e[4]*e[17]+e[9]*e[4]*e[8];
+        A[128]=-.5000000000*e[14]*ep2[16]-.5000000000*e[14]*ep2[10]-.5000000000*e[14]*ep2[9]+e[11]*e[9]*e[12]+.5000000000*ep3[14]+e[17]*e[13]*e[16]+.5000000000*e[14]*ep2[12]+e[11]*e[10]*e[13]-.5000000000*e[14]*ep2[15]+.5000000000*e[14]*ep2[17]+e[17]*e[12]*e[15]+.5000000000*ep2[11]*e[14]+.5000000000*e[14]*ep2[13];
+        A[19]=-e[21]*e[19]*e[8]+e[18]*e[4]*e[26]-e[18]*e[5]*e[25]-e[18]*e[23]*e[7]+e[21]*e[25]*e[2]-e[21]*e[1]*e[26]+e[6]*e[19]*e[23]+e[18]*e[22]*e[8]-e[0]*e[23]*e[25]-e[6]*e[22]*e[20]+e[24]*e[1]*e[23]+e[24]*e[19]*e[5]-e[24]*e[4]*e[20]-e[24]*e[22]*e[2]+e[3]*e[25]*e[20]-e[3]*e[19]*e[26]+e[0]*e[22]*e[26]+e[21]*e[7]*e[20];
+        A[129]=.5000000000*ep2[20]*e[32]+1.500000000*e[32]*ep2[23]+.5000000000*e[32]*ep2[22]+.5000000000*e[32]*ep2[21]+.5000000000*e[32]*ep2[26]-.5000000000*e[32]*ep2[18]-.5000000000*e[32]*ep2[19]-.5000000000*e[32]*ep2[24]-.5000000000*e[32]*ep2[25]+e[20]*e[27]*e[21]+e[20]*e[18]*e[30]+e[20]*e[28]*e[22]+e[20]*e[19]*e[31]+e[20]*e[29]*e[23]+e[29]*e[19]*e[22]+e[29]*e[18]*e[21]+e[23]*e[30]*e[21]+e[23]*e[31]*e[22]+e[26]*e[30]*e[24]+e[26]*e[21]*e[33]+e[26]*e[31]*e[25]+e[26]*e[22]*e[34]+e[26]*e[23]*e[35]+e[35]*e[22]*e[25]+e[35]*e[21]*e[24]-1.*e[23]*e[27]*e[18]-1.*e[23]*e[33]*e[24]-1.*e[23]*e[28]*e[19]-1.*e[23]*e[34]*e[25];
+        A[16]=-e[9]*e[23]*e[25]-e[21]*e[10]*e[26]-e[21]*e[19]*e[17]-e[18]*e[23]*e[16]+e[18]*e[13]*e[26]+e[12]*e[25]*e[20]-e[12]*e[19]*e[26]-e[15]*e[22]*e[20]+e[21]*e[16]*e[20]+e[21]*e[25]*e[11]+e[24]*e[10]*e[23]+e[24]*e[19]*e[14]-e[24]*e[13]*e[20]-e[24]*e[22]*e[11]+e[18]*e[22]*e[17]-e[18]*e[14]*e[25]+e[9]*e[22]*e[26]+e[15]*e[19]*e[23];
+        A[130]=.5000000000*e[23]*ep2[21]+e[20]*e[19]*e[22]+e[20]*e[18]*e[21]+.5000000000*ep3[23]+e[26]*e[22]*e[25]+.5000000000*e[23]*ep2[26]-.5000000000*e[23]*ep2[18]+.5000000000*e[23]*ep2[22]-.5000000000*e[23]*ep2[19]+e[26]*e[21]*e[24]+.5000000000*ep2[20]*e[23]-.5000000000*e[23]*ep2[24]-.5000000000*e[23]*ep2[25];
+        A[17]=e[18]*e[13]*e[35]-e[18]*e[32]*e[16]+e[18]*e[31]*e[17]-e[18]*e[14]*e[34]+e[27]*e[13]*e[26]+e[27]*e[22]*e[17]-e[27]*e[14]*e[25]-e[27]*e[23]*e[16]-e[9]*e[32]*e[25]+e[9]*e[22]*e[35]-e[9]*e[23]*e[34]+e[9]*e[31]*e[26]+e[15]*e[19]*e[32]+e[15]*e[28]*e[23]-e[15]*e[22]*e[29]-e[15]*e[31]*e[20]+e[24]*e[10]*e[32]+e[24]*e[28]*e[14]-e[24]*e[13]*e[29]-e[24]*e[31]*e[11]+e[33]*e[10]*e[23]+e[33]*e[19]*e[14]-e[33]*e[13]*e[20]-e[33]*e[22]*e[11]+e[21]*e[16]*e[29]-e[21]*e[10]*e[35]-e[21]*e[28]*e[17]+e[30]*e[16]*e[20]+e[30]*e[25]*e[11]-e[30]*e[10]*e[26]-e[30]*e[19]*e[17]-e[12]*e[28]*e[26]-e[12]*e[19]*e[35]+e[12]*e[34]*e[20]+e[12]*e[25]*e[29]+e[21]*e[34]*e[11];
+        A[131]=-1.*e[32]*e[10]*e[1]+e[32]*e[13]*e[4]-1.*e[32]*e[16]*e[7]-1.*e[32]*e[15]*e[6]-1.*e[32]*e[9]*e[0]+e[32]*e[12]*e[3]+e[17]*e[30]*e[6]+e[17]*e[3]*e[33]+e[17]*e[31]*e[7]+e[17]*e[4]*e[34]+e[17]*e[5]*e[35]-1.*e[5]*e[27]*e[9]-1.*e[5]*e[28]*e[10]-1.*e[5]*e[33]*e[15]-1.*e[5]*e[34]*e[16]+e[5]*e[29]*e[11]+e[35]*e[12]*e[6]+e[35]*e[3]*e[15]+e[35]*e[13]*e[7]+e[35]*e[4]*e[16]+e[11]*e[27]*e[3]+e[11]*e[0]*e[30]+e[11]*e[28]*e[4]+e[11]*e[1]*e[31]+e[29]*e[9]*e[3]+e[29]*e[0]*e[12]+e[29]*e[10]*e[4]+e[29]*e[1]*e[13]+e[5]*e[30]*e[12]+3.*e[5]*e[32]*e[14]+e[5]*e[31]*e[13]+e[8]*e[30]*e[15]+e[8]*e[12]*e[33]+e[8]*e[32]*e[17]+e[8]*e[14]*e[35]+e[8]*e[31]*e[16]+e[8]*e[13]*e[34]+e[2]*e[27]*e[12]+e[2]*e[9]*e[30]+e[2]*e[29]*e[14]+e[2]*e[11]*e[32]+e[2]*e[28]*e[13]+e[2]*e[10]*e[31]-1.*e[14]*e[27]*e[0]-1.*e[14]*e[34]*e[7]-1.*e[14]*e[33]*e[6]+e[14]*e[30]*e[3]-1.*e[14]*e[28]*e[1]+e[14]*e[31]*e[4];
+        A[22]=.5000000000*e[18]*ep2[29]+.5000000000*e[18]*ep2[28]+.5000000000*e[18]*ep2[30]+.5000000000*e[18]*ep2[33]-.5000000000*e[18]*ep2[32]-.5000000000*e[18]*ep2[31]-.5000000000*e[18]*ep2[34]-.5000000000*e[18]*ep2[35]+1.500000000*e[18]*ep2[27]+e[27]*e[28]*e[19]+e[27]*e[29]*e[20]+e[21]*e[27]*e[30]+e[21]*e[29]*e[32]+e[21]*e[28]*e[31]+e[30]*e[28]*e[22]+e[30]*e[19]*e[31]+e[30]*e[29]*e[23]+e[30]*e[20]*e[32]+e[24]*e[27]*e[33]+e[24]*e[29]*e[35]+e[24]*e[28]*e[34]+e[33]*e[28]*e[25]+e[33]*e[19]*e[34]+e[33]*e[29]*e[26]+e[33]*e[20]*e[35]-1.*e[27]*e[35]*e[26]-1.*e[27]*e[31]*e[22]-1.*e[27]*e[32]*e[23]-1.*e[27]*e[34]*e[25];
+        A[132]=e[20]*e[1]*e[4]+e[20]*e[0]*e[3]+e[20]*e[2]*e[5]+e[5]*e[21]*e[3]+e[5]*e[22]*e[4]+e[8]*e[21]*e[6]+e[8]*e[3]*e[24]+e[8]*e[22]*e[7]+e[8]*e[4]*e[25]+e[8]*e[5]*e[26]+e[26]*e[4]*e[7]+e[26]*e[3]*e[6]+e[2]*e[18]*e[3]+e[2]*e[0]*e[21]+e[2]*e[19]*e[4]+e[2]*e[1]*e[22]-1.*e[5]*e[19]*e[1]-1.*e[5]*e[18]*e[0]-1.*e[5]*e[25]*e[7]-1.*e[5]*e[24]*e[6]+.5000000000*e[23]*ep2[4]-.5000000000*e[23]*ep2[0]-.5000000000*e[23]*ep2[6]+1.500000000*e[23]*ep2[5]-.5000000000*e[23]*ep2[1]-.5000000000*e[23]*ep2[7]+.5000000000*e[23]*ep2[3]+.5000000000*e[23]*ep2[2]+.5000000000*e[23]*ep2[8];
+        A[23]=1.500000000*e[9]*ep2[27]+.5000000000*e[9]*ep2[29]+.5000000000*e[9]*ep2[28]-.5000000000*e[9]*ep2[32]-.5000000000*e[9]*ep2[31]+.5000000000*e[9]*ep2[33]+.5000000000*e[9]*ep2[30]-.5000000000*e[9]*ep2[34]-.5000000000*e[9]*ep2[35]+e[33]*e[27]*e[15]+e[33]*e[29]*e[17]+e[33]*e[11]*e[35]+e[33]*e[28]*e[16]+e[33]*e[10]*e[34]+e[27]*e[29]*e[11]+e[27]*e[28]*e[10]+e[27]*e[30]*e[12]-1.*e[27]*e[31]*e[13]-1.*e[27]*e[32]*e[14]-1.*e[27]*e[34]*e[16]-1.*e[27]*e[35]*e[17]+e[30]*e[29]*e[14]+e[30]*e[11]*e[32]+e[30]*e[28]*e[13]+e[30]*e[10]*e[31]+e[12]*e[29]*e[32]+e[12]*e[28]*e[31]+e[15]*e[29]*e[35]+e[15]*e[28]*e[34];
+        A[133]=-1.*e[32]*e[24]*e[6]+e[8]*e[30]*e[24]+e[8]*e[21]*e[33]+e[8]*e[31]*e[25]+e[8]*e[22]*e[34]+e[26]*e[30]*e[6]+e[26]*e[3]*e[33]+e[26]*e[31]*e[7]+e[26]*e[4]*e[34]+e[26]*e[32]*e[8]+e[26]*e[5]*e[35]+e[35]*e[21]*e[6]+e[35]*e[3]*e[24]+e[35]*e[22]*e[7]+e[35]*e[4]*e[25]+e[35]*e[23]*e[8]+e[2]*e[27]*e[21]+e[2]*e[18]*e[30]+e[2]*e[28]*e[22]+e[2]*e[19]*e[31]+e[2]*e[29]*e[23]+e[2]*e[20]*e[32]+e[20]*e[27]*e[3]+e[20]*e[0]*e[30]+e[20]*e[28]*e[4]+e[20]*e[1]*e[31]+e[20]*e[29]*e[5]+e[29]*e[18]*e[3]+e[29]*e[0]*e[21]+e[29]*e[19]*e[4]+e[29]*e[1]*e[22]+e[5]*e[30]*e[21]+e[5]*e[31]*e[22]+3.*e[5]*e[32]*e[23]-1.*e[5]*e[27]*e[18]-1.*e[5]*e[33]*e[24]-1.*e[5]*e[28]*e[19]-1.*e[5]*e[34]*e[25]-1.*e[23]*e[27]*e[0]-1.*e[23]*e[34]*e[7]-1.*e[23]*e[33]*e[6]+e[23]*e[30]*e[3]-1.*e[23]*e[28]*e[1]+e[23]*e[31]*e[4]+e[32]*e[21]*e[3]-1.*e[32]*e[19]*e[1]+e[32]*e[22]*e[4]-1.*e[32]*e[18]*e[0]-1.*e[32]*e[25]*e[7];
+        A[20]=.5000000000*e[27]*ep2[33]-.5000000000*e[27]*ep2[32]-.5000000000*e[27]*ep2[31]-.5000000000*e[27]*ep2[34]-.5000000000*e[27]*ep2[35]+e[33]*e[29]*e[35]+.5000000000*e[27]*ep2[29]+e[30]*e[29]*e[32]+e[30]*e[28]*e[31]+e[33]*e[28]*e[34]+.5000000000*e[27]*ep2[28]+.5000000000*e[27]*ep2[30]+.5000000000*ep3[27];
+        A[134]=e[14]*e[21]*e[12]+e[14]*e[22]*e[13]+e[17]*e[21]*e[15]+e[17]*e[12]*e[24]+e[17]*e[14]*e[26]+e[17]*e[22]*e[16]+e[17]*e[13]*e[25]+e[26]*e[12]*e[15]+e[26]*e[13]*e[16]-1.*e[14]*e[24]*e[15]-1.*e[14]*e[25]*e[16]-1.*e[14]*e[18]*e[9]-1.*e[14]*e[19]*e[10]+e[11]*e[18]*e[12]+e[11]*e[9]*e[21]+e[11]*e[19]*e[13]+e[11]*e[10]*e[22]+e[20]*e[11]*e[14]+e[20]*e[9]*e[12]+e[20]*e[10]*e[13]+1.500000000*e[23]*ep2[14]+.5000000000*e[23]*ep2[12]+.5000000000*e[23]*ep2[13]+.5000000000*e[23]*ep2[17]+.5000000000*ep2[11]*e[23]-.5000000000*e[23]*ep2[16]-.5000000000*e[23]*ep2[9]-.5000000000*e[23]*ep2[15]-.5000000000*e[23]*ep2[10];
+        A[21]=1.500000000*e[0]*ep2[27]+.5000000000*e[0]*ep2[29]+.5000000000*e[0]*ep2[28]+.5000000000*e[0]*ep2[30]-.5000000000*e[0]*ep2[32]-.5000000000*e[0]*ep2[31]+.5000000000*e[0]*ep2[33]-.5000000000*e[0]*ep2[34]-.5000000000*e[0]*ep2[35]-1.*e[27]*e[31]*e[4]+e[3]*e[27]*e[30]+e[3]*e[29]*e[32]+e[3]*e[28]*e[31]+e[30]*e[28]*e[4]+e[30]*e[1]*e[31]+e[30]*e[29]*e[5]+e[30]*e[2]*e[32]+e[6]*e[27]*e[33]+e[6]*e[29]*e[35]+e[6]*e[28]*e[34]+e[27]*e[28]*e[1]+e[27]*e[29]*e[2]+e[33]*e[28]*e[7]+e[33]*e[1]*e[34]+e[33]*e[29]*e[8]+e[33]*e[2]*e[35]-1.*e[27]*e[34]*e[7]-1.*e[27]*e[32]*e[5]-1.*e[27]*e[35]*e[8];
+        A[135]=e[14]*e[12]*e[3]+e[14]*e[13]*e[4]+e[17]*e[12]*e[6]+e[17]*e[3]*e[15]+e[17]*e[13]*e[7]+e[17]*e[4]*e[16]+e[17]*e[14]*e[8]+e[8]*e[12]*e[15]+e[8]*e[13]*e[16]+e[2]*e[11]*e[14]+e[2]*e[9]*e[12]+e[2]*e[10]*e[13]+e[11]*e[9]*e[3]+e[11]*e[0]*e[12]+e[11]*e[10]*e[4]+e[11]*e[1]*e[13]-1.*e[14]*e[10]*e[1]-1.*e[14]*e[16]*e[7]-1.*e[14]*e[15]*e[6]-1.*e[14]*e[9]*e[0]-.5000000000*e[5]*ep2[16]-.5000000000*e[5]*ep2[9]+.5000000000*e[5]*ep2[11]+.5000000000*e[5]*ep2[12]-.5000000000*e[5]*ep2[15]-.5000000000*e[5]*ep2[10]+.5000000000*e[5]*ep2[13]+1.500000000*ep2[14]*e[5]+.5000000000*e[5]*ep2[17];
+        A[27]=1.500000000*e[27]*ep2[9]-.5000000000*e[27]*ep2[16]+.5000000000*e[27]*ep2[11]+.5000000000*e[27]*ep2[12]+.5000000000*e[27]*ep2[15]-.5000000000*e[27]*ep2[17]+.5000000000*e[27]*ep2[10]-.5000000000*e[27]*ep2[14]-.5000000000*e[27]*ep2[13]+e[12]*e[10]*e[31]+e[30]*e[11]*e[14]+e[30]*e[10]*e[13]+e[15]*e[9]*e[33]+e[15]*e[29]*e[17]+e[15]*e[11]*e[35]+e[15]*e[28]*e[16]+e[15]*e[10]*e[34]+e[33]*e[11]*e[17]+e[33]*e[10]*e[16]-1.*e[9]*e[31]*e[13]-1.*e[9]*e[32]*e[14]-1.*e[9]*e[34]*e[16]-1.*e[9]*e[35]*e[17]+e[9]*e[29]*e[11]+e[9]*e[28]*e[10]+e[12]*e[9]*e[30]+e[12]*e[29]*e[14]+e[12]*e[11]*e[32]+e[12]*e[28]*e[13];
+        A[137]=e[29]*e[18]*e[12]+e[29]*e[9]*e[21]+e[29]*e[19]*e[13]+e[29]*e[10]*e[22]+e[17]*e[30]*e[24]+e[17]*e[21]*e[33]+e[17]*e[31]*e[25]+e[17]*e[22]*e[34]+e[17]*e[32]*e[26]+e[17]*e[23]*e[35]-1.*e[23]*e[27]*e[9]-1.*e[23]*e[28]*e[10]-1.*e[23]*e[33]*e[15]-1.*e[23]*e[34]*e[16]-1.*e[32]*e[24]*e[15]-1.*e[32]*e[25]*e[16]-1.*e[32]*e[18]*e[9]-1.*e[32]*e[19]*e[10]+e[26]*e[30]*e[15]+e[26]*e[12]*e[33]+e[26]*e[31]*e[16]+e[26]*e[13]*e[34]+e[35]*e[21]*e[15]+e[35]*e[12]*e[24]+e[35]*e[22]*e[16]+e[35]*e[13]*e[25]+e[14]*e[30]*e[21]+e[14]*e[31]*e[22]+3.*e[14]*e[32]*e[23]+e[11]*e[27]*e[21]+e[11]*e[18]*e[30]+e[11]*e[28]*e[22]+e[11]*e[19]*e[31]+e[11]*e[29]*e[23]+e[11]*e[20]*e[32]+e[23]*e[30]*e[12]+e[23]*e[31]*e[13]+e[32]*e[21]*e[12]+e[32]*e[22]*e[13]-1.*e[14]*e[27]*e[18]-1.*e[14]*e[33]*e[24]+e[14]*e[29]*e[20]+e[14]*e[35]*e[26]-1.*e[14]*e[28]*e[19]-1.*e[14]*e[34]*e[25]+e[20]*e[27]*e[12]+e[20]*e[9]*e[30]+e[20]*e[28]*e[13]+e[20]*e[10]*e[31];
+        A[26]=.5000000000*e[0]*ep2[1]+.5000000000*e[0]*ep2[2]+e[6]*e[2]*e[8]+e[6]*e[1]*e[7]+.5000000000*e[0]*ep2[3]+e[3]*e[1]*e[4]+.5000000000*e[0]*ep2[6]+e[3]*e[2]*e[5]-.5000000000*e[0]*ep2[5]-.5000000000*e[0]*ep2[8]+.5000000000*ep3[0]-.5000000000*e[0]*ep2[7]-.5000000000*e[0]*ep2[4];
+        A[136]=1.500000000*ep2[23]*e[14]+.5000000000*e[14]*ep2[26]-.5000000000*e[14]*ep2[18]-.5000000000*e[14]*ep2[19]+.5000000000*e[14]*ep2[20]+.5000000000*e[14]*ep2[22]-.5000000000*e[14]*ep2[24]+.5000000000*e[14]*ep2[21]-.5000000000*e[14]*ep2[25]+e[23]*e[21]*e[12]+e[23]*e[22]*e[13]+e[26]*e[21]*e[15]+e[26]*e[12]*e[24]+e[26]*e[23]*e[17]+e[26]*e[22]*e[16]+e[26]*e[13]*e[25]+e[17]*e[22]*e[25]+e[17]*e[21]*e[24]+e[11]*e[19]*e[22]+e[11]*e[18]*e[21]+e[11]*e[20]*e[23]+e[20]*e[18]*e[12]+e[20]*e[9]*e[21]+e[20]*e[19]*e[13]+e[20]*e[10]*e[22]-1.*e[23]*e[24]*e[15]-1.*e[23]*e[25]*e[16]-1.*e[23]*e[18]*e[9]-1.*e[23]*e[19]*e[10];
+        A[25]=1.500000000*e[27]*ep2[0]-.5000000000*e[27]*ep2[4]+.5000000000*e[27]*ep2[6]-.5000000000*e[27]*ep2[5]+.5000000000*e[27]*ep2[1]-.5000000000*e[27]*ep2[7]+.5000000000*e[27]*ep2[3]+.5000000000*e[27]*ep2[2]-.5000000000*e[27]*ep2[8]+e[0]*e[33]*e[6]+e[0]*e[30]*e[3]-1.*e[0]*e[35]*e[8]-1.*e[0]*e[31]*e[4]+e[3]*e[28]*e[4]+e[3]*e[1]*e[31]+e[3]*e[29]*e[5]+e[3]*e[2]*e[32]+e[30]*e[1]*e[4]+e[30]*e[2]*e[5]+e[6]*e[28]*e[7]+e[6]*e[1]*e[34]+e[6]*e[29]*e[8]+e[6]*e[2]*e[35]+e[33]*e[1]*e[7]+e[33]*e[2]*e[8]+e[0]*e[28]*e[1]+e[0]*e[29]*e[2]-1.*e[0]*e[34]*e[7]-1.*e[0]*e[32]*e[5];
+        A[139]=e[8]*e[22]*e[25]+e[8]*e[21]*e[24]+e[20]*e[18]*e[3]+e[20]*e[0]*e[21]+e[20]*e[19]*e[4]+e[20]*e[1]*e[22]+e[20]*e[2]*e[23]+e[23]*e[21]*e[3]+e[23]*e[22]*e[4]+e[23]*e[26]*e[8]-1.*e[23]*e[19]*e[1]-1.*e[23]*e[18]*e[0]-1.*e[23]*e[25]*e[7]-1.*e[23]*e[24]*e[6]+e[2]*e[19]*e[22]+e[2]*e[18]*e[21]+e[26]*e[21]*e[6]+e[26]*e[3]*e[24]+e[26]*e[22]*e[7]+e[26]*e[4]*e[25]+.5000000000*ep2[20]*e[5]+1.500000000*ep2[23]*e[5]+.5000000000*e[5]*ep2[22]+.5000000000*e[5]*ep2[21]+.5000000000*e[5]*ep2[26]-.5000000000*e[5]*ep2[18]-.5000000000*e[5]*ep2[19]-.5000000000*e[5]*ep2[24]-.5000000000*e[5]*ep2[25];
+        A[24]=e[24]*e[11]*e[8]+e[24]*e[2]*e[17]+3.*e[9]*e[18]*e[0]+e[9]*e[19]*e[1]+e[9]*e[20]*e[2]+e[18]*e[10]*e[1]+e[18]*e[11]*e[2]+e[3]*e[18]*e[12]+e[3]*e[9]*e[21]+e[3]*e[20]*e[14]+e[3]*e[11]*e[23]+e[3]*e[19]*e[13]+e[3]*e[10]*e[22]+e[6]*e[18]*e[15]+e[6]*e[9]*e[24]+e[6]*e[20]*e[17]+e[6]*e[11]*e[26]+e[6]*e[19]*e[16]+e[6]*e[10]*e[25]+e[0]*e[20]*e[11]+e[0]*e[19]*e[10]-1.*e[9]*e[26]*e[8]-1.*e[9]*e[22]*e[4]-1.*e[9]*e[25]*e[7]-1.*e[9]*e[23]*e[5]+e[12]*e[0]*e[21]+e[12]*e[19]*e[4]+e[12]*e[1]*e[22]+e[12]*e[20]*e[5]+e[12]*e[2]*e[23]-1.*e[18]*e[13]*e[4]-1.*e[18]*e[16]*e[7]-1.*e[18]*e[14]*e[5]-1.*e[18]*e[17]*e[8]+e[21]*e[10]*e[4]+e[21]*e[1]*e[13]+e[21]*e[11]*e[5]+e[21]*e[2]*e[14]+e[15]*e[0]*e[24]+e[15]*e[19]*e[7]+e[15]*e[1]*e[25]+e[15]*e[20]*e[8]+e[15]*e[2]*e[26]-1.*e[0]*e[23]*e[14]-1.*e[0]*e[25]*e[16]-1.*e[0]*e[26]*e[17]-1.*e[0]*e[22]*e[13]+e[24]*e[10]*e[7]+e[24]*e[1]*e[16];
+        A[138]=e[11]*e[1]*e[4]+e[11]*e[0]*e[3]+e[11]*e[2]*e[5]+e[5]*e[12]*e[3]+e[5]*e[13]*e[4]+e[8]*e[12]*e[6]+e[8]*e[3]*e[15]+e[8]*e[13]*e[7]+e[8]*e[4]*e[16]+e[8]*e[5]*e[17]+e[17]*e[4]*e[7]+e[17]*e[3]*e[6]-1.*e[5]*e[10]*e[1]-1.*e[5]*e[16]*e[7]-1.*e[5]*e[15]*e[6]-1.*e[5]*e[9]*e[0]+e[2]*e[9]*e[3]+e[2]*e[0]*e[12]+e[2]*e[10]*e[4]+e[2]*e[1]*e[13]+.5000000000*ep2[2]*e[14]-.5000000000*e[14]*ep2[0]-.5000000000*e[14]*ep2[6]-.5000000000*e[14]*ep2[1]-.5000000000*e[14]*ep2[7]+1.500000000*e[14]*ep2[5]+.5000000000*e[14]*ep2[4]+.5000000000*e[14]*ep2[3]+.5000000000*e[14]*ep2[8];
+        A[31]=e[3]*e[27]*e[12]+e[3]*e[9]*e[30]+e[3]*e[29]*e[14]+e[3]*e[11]*e[32]+e[3]*e[28]*e[13]+e[3]*e[10]*e[31]+e[6]*e[27]*e[15]+e[6]*e[9]*e[33]+e[6]*e[29]*e[17]+e[6]*e[11]*e[35]+e[6]*e[28]*e[16]+e[6]*e[10]*e[34]+3.*e[0]*e[27]*e[9]+e[0]*e[29]*e[11]+e[0]*e[28]*e[10]-1.*e[9]*e[34]*e[7]-1.*e[9]*e[32]*e[5]-1.*e[9]*e[35]*e[8]+e[9]*e[29]*e[2]+e[9]*e[28]*e[1]-1.*e[9]*e[31]*e[4]+e[12]*e[0]*e[30]+e[12]*e[28]*e[4]+e[12]*e[1]*e[31]+e[12]*e[29]*e[5]+e[12]*e[2]*e[32]+e[27]*e[11]*e[2]+e[27]*e[10]*e[1]-1.*e[27]*e[13]*e[4]-1.*e[27]*e[16]*e[7]-1.*e[27]*e[14]*e[5]-1.*e[27]*e[17]*e[8]+e[30]*e[10]*e[4]+e[30]*e[1]*e[13]+e[30]*e[11]*e[5]+e[30]*e[2]*e[14]+e[15]*e[0]*e[33]+e[15]*e[28]*e[7]+e[15]*e[1]*e[34]+e[15]*e[29]*e[8]+e[15]*e[2]*e[35]-1.*e[0]*e[31]*e[13]-1.*e[0]*e[32]*e[14]-1.*e[0]*e[34]*e[16]-1.*e[0]*e[35]*e[17]+e[33]*e[10]*e[7]+e[33]*e[1]*e[16]+e[33]*e[11]*e[8]+e[33]*e[2]*e[17];
+        A[141]=.5000000000*ep2[30]*e[6]+.5000000000*e[6]*ep2[27]-.5000000000*e[6]*ep2[32]-.5000000000*e[6]*ep2[28]-.5000000000*e[6]*ep2[29]-.5000000000*e[6]*ep2[31]+1.500000000*e[6]*ep2[33]+.5000000000*e[6]*ep2[34]+.5000000000*e[6]*ep2[35]+e[0]*e[27]*e[33]+e[0]*e[29]*e[35]+e[0]*e[28]*e[34]+e[3]*e[30]*e[33]+e[3]*e[32]*e[35]+e[3]*e[31]*e[34]+e[30]*e[31]*e[7]+e[30]*e[4]*e[34]+e[30]*e[32]*e[8]+e[30]*e[5]*e[35]+e[27]*e[28]*e[7]+e[27]*e[1]*e[34]+e[27]*e[29]*e[8]+e[27]*e[2]*e[35]+e[33]*e[34]*e[7]+e[33]*e[35]*e[8]-1.*e[33]*e[32]*e[5]-1.*e[33]*e[29]*e[2]-1.*e[33]*e[28]*e[1]-1.*e[33]*e[31]*e[4];
+        A[30]=e[24]*e[20]*e[26]+e[21]*e[19]*e[22]-.5000000000*e[18]*ep2[22]-.5000000000*e[18]*ep2[25]+.5000000000*ep3[18]+.5000000000*e[18]*ep2[21]+e[21]*e[20]*e[23]+.5000000000*e[18]*ep2[20]+.5000000000*e[18]*ep2[19]+.5000000000*e[18]*ep2[24]+e[24]*e[19]*e[25]-.5000000000*e[18]*ep2[23]-.5000000000*e[18]*ep2[26];
+        A[140]=.5000000000*e[33]*ep2[35]+.5000000000*ep3[33]+.5000000000*ep2[27]*e[33]+.5000000000*ep2[30]*e[33]-.5000000000*e[33]*ep2[29]+.5000000000*e[33]*ep2[34]-.5000000000*e[33]*ep2[32]-.5000000000*e[33]*ep2[28]+e[30]*e[32]*e[35]-.5000000000*e[33]*ep2[31]+e[27]*e[29]*e[35]+e[27]*e[28]*e[34]+e[30]*e[31]*e[34];
+        A[29]=1.500000000*e[27]*ep2[18]+.5000000000*e[27]*ep2[19]+.5000000000*e[27]*ep2[20]+.5000000000*e[27]*ep2[21]+.5000000000*e[27]*ep2[24]-.5000000000*e[27]*ep2[26]-.5000000000*e[27]*ep2[23]-.5000000000*e[27]*ep2[22]-.5000000000*e[27]*ep2[25]+e[33]*e[20]*e[26]-1.*e[18]*e[35]*e[26]-1.*e[18]*e[31]*e[22]-1.*e[18]*e[32]*e[23]-1.*e[18]*e[34]*e[25]+e[18]*e[28]*e[19]+e[18]*e[29]*e[20]+e[21]*e[18]*e[30]+e[21]*e[28]*e[22]+e[21]*e[19]*e[31]+e[21]*e[29]*e[23]+e[21]*e[20]*e[32]+e[30]*e[19]*e[22]+e[30]*e[20]*e[23]+e[24]*e[18]*e[33]+e[24]*e[28]*e[25]+e[24]*e[19]*e[34]+e[24]*e[29]*e[26]+e[24]*e[20]*e[35]+e[33]*e[19]*e[25];
+        A[143]=e[9]*e[27]*e[33]+e[9]*e[29]*e[35]+e[9]*e[28]*e[34]+e[33]*e[35]*e[17]+e[33]*e[34]*e[16]+e[27]*e[29]*e[17]+e[27]*e[11]*e[35]+e[27]*e[28]*e[16]+e[27]*e[10]*e[34]+e[33]*e[30]*e[12]-1.*e[33]*e[28]*e[10]-1.*e[33]*e[31]*e[13]-1.*e[33]*e[32]*e[14]-1.*e[33]*e[29]*e[11]+e[30]*e[32]*e[17]+e[30]*e[14]*e[35]+e[30]*e[31]*e[16]+e[30]*e[13]*e[34]+e[12]*e[32]*e[35]+e[12]*e[31]*e[34]+.5000000000*e[15]*ep2[27]-.5000000000*e[15]*ep2[32]-.5000000000*e[15]*ep2[28]-.5000000000*e[15]*ep2[29]-.5000000000*e[15]*ep2[31]+1.500000000*e[15]*ep2[33]+.5000000000*e[15]*ep2[30]+.5000000000*e[15]*ep2[34]+.5000000000*e[15]*ep2[35];
+        A[28]=.5000000000*e[9]*ep2[12]-.5000000000*e[9]*ep2[16]+.5000000000*e[9]*ep2[10]-.5000000000*e[9]*ep2[17]-.5000000000*e[9]*ep2[13]+e[15]*e[10]*e[16]+e[12]*e[11]*e[14]+.5000000000*e[9]*ep2[11]+.5000000000*e[9]*ep2[15]-.5000000000*e[9]*ep2[14]+e[15]*e[11]*e[17]+.5000000000*ep3[9]+e[12]*e[10]*e[13];
+        A[142]=e[18]*e[27]*e[33]+e[18]*e[29]*e[35]+e[18]*e[28]*e[34]+e[27]*e[28]*e[25]+e[27]*e[19]*e[34]+e[27]*e[29]*e[26]+e[27]*e[20]*e[35]+e[21]*e[30]*e[33]+e[21]*e[32]*e[35]+e[21]*e[31]*e[34]+e[30]*e[31]*e[25]+e[30]*e[22]*e[34]+e[30]*e[32]*e[26]+e[30]*e[23]*e[35]+e[33]*e[34]*e[25]+e[33]*e[35]*e[26]-1.*e[33]*e[29]*e[20]-1.*e[33]*e[31]*e[22]-1.*e[33]*e[32]*e[23]-1.*e[33]*e[28]*e[19]+.5000000000*ep2[27]*e[24]+.5000000000*ep2[30]*e[24]+1.500000000*e[24]*ep2[33]+.5000000000*e[24]*ep2[35]+.5000000000*e[24]*ep2[34]-.5000000000*e[24]*ep2[32]-.5000000000*e[24]*ep2[28]-.5000000000*e[24]*ep2[29]-.5000000000*e[24]*ep2[31];
+        A[36]=.5000000000*e[9]*ep2[21]+.5000000000*e[9]*ep2[24]+.5000000000*e[9]*ep2[19]+1.500000000*e[9]*ep2[18]+.5000000000*e[9]*ep2[20]-.5000000000*e[9]*ep2[26]-.5000000000*e[9]*ep2[23]-.5000000000*e[9]*ep2[22]-.5000000000*e[9]*ep2[25]+e[21]*e[18]*e[12]+e[21]*e[20]*e[14]+e[21]*e[11]*e[23]+e[21]*e[19]*e[13]+e[21]*e[10]*e[22]+e[24]*e[18]*e[15]+e[24]*e[20]*e[17]+e[24]*e[11]*e[26]+e[24]*e[19]*e[16]+e[24]*e[10]*e[25]+e[15]*e[19]*e[25]+e[15]*e[20]*e[26]+e[12]*e[19]*e[22]+e[12]*e[20]*e[23]+e[18]*e[20]*e[11]+e[18]*e[19]*e[10]-1.*e[18]*e[23]*e[14]-1.*e[18]*e[25]*e[16]-1.*e[18]*e[26]*e[17]-1.*e[18]*e[22]*e[13];
+        A[182]=.5000000000*ep2[29]*e[26]+.5000000000*ep2[32]*e[26]+.5000000000*e[26]*ep2[33]+1.500000000*e[26]*ep2[35]+.5000000000*e[26]*ep2[34]-.5000000000*e[26]*ep2[27]-.5000000000*e[26]*ep2[28]-.5000000000*e[26]*ep2[31]-.5000000000*e[26]*ep2[30]+e[20]*e[27]*e[33]+e[20]*e[29]*e[35]+e[20]*e[28]*e[34]+e[29]*e[27]*e[24]+e[29]*e[18]*e[33]+e[29]*e[28]*e[25]+e[29]*e[19]*e[34]+e[23]*e[30]*e[33]+e[23]*e[32]*e[35]+e[23]*e[31]*e[34]+e[32]*e[30]*e[24]+e[32]*e[21]*e[33]+e[32]*e[31]*e[25]+e[32]*e[22]*e[34]+e[35]*e[33]*e[24]+e[35]*e[34]*e[25]-1.*e[35]*e[27]*e[18]-1.*e[35]*e[30]*e[21]-1.*e[35]*e[31]*e[22]-1.*e[35]*e[28]*e[19];
+        A[37]=e[12]*e[19]*e[31]+e[12]*e[29]*e[23]+e[12]*e[20]*e[32]+3.*e[9]*e[27]*e[18]+e[9]*e[28]*e[19]+e[9]*e[29]*e[20]+e[21]*e[9]*e[30]+e[21]*e[29]*e[14]+e[21]*e[11]*e[32]+e[21]*e[28]*e[13]+e[21]*e[10]*e[31]+e[30]*e[20]*e[14]+e[30]*e[11]*e[23]+e[30]*e[19]*e[13]+e[30]*e[10]*e[22]+e[9]*e[33]*e[24]-1.*e[9]*e[35]*e[26]-1.*e[9]*e[31]*e[22]-1.*e[9]*e[32]*e[23]-1.*e[9]*e[34]*e[25]+e[18]*e[29]*e[11]+e[18]*e[28]*e[10]+e[27]*e[20]*e[11]+e[27]*e[19]*e[10]+e[15]*e[27]*e[24]+e[15]*e[18]*e[33]+e[15]*e[28]*e[25]+e[15]*e[19]*e[34]+e[15]*e[29]*e[26]+e[15]*e[20]*e[35]-1.*e[18]*e[31]*e[13]-1.*e[18]*e[32]*e[14]-1.*e[18]*e[34]*e[16]-1.*e[18]*e[35]*e[17]-1.*e[27]*e[23]*e[14]-1.*e[27]*e[25]*e[16]-1.*e[27]*e[26]*e[17]-1.*e[27]*e[22]*e[13]+e[24]*e[29]*e[17]+e[24]*e[11]*e[35]+e[24]*e[28]*e[16]+e[24]*e[10]*e[34]+e[33]*e[20]*e[17]+e[33]*e[11]*e[26]+e[33]*e[19]*e[16]+e[33]*e[10]*e[25]+e[12]*e[27]*e[21]+e[12]*e[18]*e[30]+e[12]*e[28]*e[22];
+        A[183]=-.5000000000*e[17]*ep2[27]+.5000000000*e[17]*ep2[32]-.5000000000*e[17]*ep2[28]+.5000000000*e[17]*ep2[29]-.5000000000*e[17]*ep2[31]+.5000000000*e[17]*ep2[33]-.5000000000*e[17]*ep2[30]+.5000000000*e[17]*ep2[34]+1.500000000*e[17]*ep2[35]+e[32]*e[30]*e[15]+e[32]*e[12]*e[33]+e[32]*e[31]*e[16]+e[32]*e[13]*e[34]+e[14]*e[30]*e[33]+e[14]*e[31]*e[34]+e[11]*e[27]*e[33]+e[11]*e[29]*e[35]+e[11]*e[28]*e[34]+e[35]*e[33]*e[15]+e[35]*e[34]*e[16]+e[29]*e[27]*e[15]+e[29]*e[9]*e[33]+e[29]*e[28]*e[16]+e[29]*e[10]*e[34]-1.*e[35]*e[27]*e[9]-1.*e[35]*e[30]*e[12]-1.*e[35]*e[28]*e[10]-1.*e[35]*e[31]*e[13]+e[35]*e[32]*e[14];
+        A[38]=.5000000000*e[9]*ep2[1]+1.500000000*e[9]*ep2[0]+.5000000000*e[9]*ep2[2]+.5000000000*e[9]*ep2[3]+.5000000000*e[9]*ep2[6]-.5000000000*e[9]*ep2[4]-.5000000000*e[9]*ep2[5]-.5000000000*e[9]*ep2[7]-.5000000000*e[9]*ep2[8]+e[6]*e[0]*e[15]+e[6]*e[10]*e[7]+e[6]*e[1]*e[16]+e[6]*e[11]*e[8]+e[6]*e[2]*e[17]+e[15]*e[1]*e[7]+e[15]*e[2]*e[8]+e[0]*e[11]*e[2]+e[0]*e[10]*e[1]-1.*e[0]*e[13]*e[4]-1.*e[0]*e[16]*e[7]-1.*e[0]*e[14]*e[5]-1.*e[0]*e[17]*e[8]+e[3]*e[0]*e[12]+e[3]*e[10]*e[4]+e[3]*e[1]*e[13]+e[3]*e[11]*e[5]+e[3]*e[2]*e[14]+e[12]*e[1]*e[4]+e[12]*e[2]*e[5];
+        A[180]=.5000000000*e[35]*ep2[33]+.5000000000*e[35]*ep2[34]-.5000000000*e[35]*ep2[27]-.5000000000*e[35]*ep2[28]-.5000000000*e[35]*ep2[31]-.5000000000*e[35]*ep2[30]+e[32]*e[31]*e[34]+.5000000000*ep2[29]*e[35]+.5000000000*ep2[32]*e[35]+e[29]*e[28]*e[34]+e[32]*e[30]*e[33]+.5000000000*ep3[35]+e[29]*e[27]*e[33];
+        A[39]=.5000000000*e[0]*ep2[19]+.5000000000*e[0]*ep2[20]+.5000000000*e[0]*ep2[24]-.5000000000*e[0]*ep2[26]-.5000000000*e[0]*ep2[23]-.5000000000*e[0]*ep2[22]-.5000000000*e[0]*ep2[25]+1.500000000*ep2[18]*e[0]+.5000000000*e[0]*ep2[21]+e[18]*e[19]*e[1]+e[18]*e[20]*e[2]+e[21]*e[18]*e[3]+e[21]*e[19]*e[4]+e[21]*e[1]*e[22]+e[21]*e[20]*e[5]+e[21]*e[2]*e[23]-1.*e[18]*e[26]*e[8]-1.*e[18]*e[22]*e[4]-1.*e[18]*e[25]*e[7]-1.*e[18]*e[23]*e[5]+e[18]*e[24]*e[6]+e[3]*e[19]*e[22]+e[3]*e[20]*e[23]+e[24]*e[19]*e[7]+e[24]*e[1]*e[25]+e[24]*e[20]*e[8]+e[24]*e[2]*e[26]+e[6]*e[19]*e[25]+e[6]*e[20]*e[26];
+        A[181]=.5000000000*ep2[32]*e[8]-.5000000000*e[8]*ep2[27]-.5000000000*e[8]*ep2[28]+.5000000000*e[8]*ep2[29]-.5000000000*e[8]*ep2[31]+.5000000000*e[8]*ep2[33]-.5000000000*e[8]*ep2[30]+.5000000000*e[8]*ep2[34]+1.500000000*e[8]*ep2[35]+e[2]*e[27]*e[33]+e[2]*e[29]*e[35]+e[2]*e[28]*e[34]+e[5]*e[30]*e[33]+e[5]*e[32]*e[35]+e[5]*e[31]*e[34]+e[32]*e[30]*e[6]+e[32]*e[3]*e[33]+e[32]*e[31]*e[7]+e[32]*e[4]*e[34]+e[29]*e[27]*e[6]+e[29]*e[0]*e[33]+e[29]*e[28]*e[7]+e[29]*e[1]*e[34]+e[35]*e[33]*e[6]+e[35]*e[34]*e[7]-1.*e[35]*e[27]*e[0]-1.*e[35]*e[30]*e[3]-1.*e[35]*e[28]*e[1]-1.*e[35]*e[31]*e[4];
+        A[32]=-.5000000000*e[18]*ep2[4]+1.500000000*e[18]*ep2[0]+.5000000000*e[18]*ep2[6]-.5000000000*e[18]*ep2[5]+.5000000000*e[18]*ep2[1]-.5000000000*e[18]*ep2[7]+.5000000000*e[18]*ep2[3]+.5000000000*e[18]*ep2[2]-.5000000000*e[18]*ep2[8]+e[3]*e[0]*e[21]+e[3]*e[19]*e[4]+e[3]*e[1]*e[22]+e[3]*e[20]*e[5]+e[3]*e[2]*e[23]+e[21]*e[1]*e[4]+e[21]*e[2]*e[5]+e[6]*e[0]*e[24]+e[6]*e[19]*e[7]+e[6]*e[1]*e[25]+e[6]*e[20]*e[8]+e[6]*e[2]*e[26]+e[24]*e[1]*e[7]+e[24]*e[2]*e[8]+e[0]*e[19]*e[1]+e[0]*e[20]*e[2]-1.*e[0]*e[26]*e[8]-1.*e[0]*e[22]*e[4]-1.*e[0]*e[25]*e[7]-1.*e[0]*e[23]*e[5];
+        A[178]=e[10]*e[1]*e[7]+e[10]*e[0]*e[6]+e[10]*e[2]*e[8]+e[4]*e[12]*e[6]+e[4]*e[3]*e[15]+e[4]*e[13]*e[7]+e[4]*e[14]*e[8]+e[4]*e[5]*e[17]+e[13]*e[3]*e[6]+e[13]*e[5]*e[8]+e[7]*e[15]*e[6]+e[7]*e[17]*e[8]-1.*e[7]*e[11]*e[2]-1.*e[7]*e[9]*e[0]-1.*e[7]*e[14]*e[5]-1.*e[7]*e[12]*e[3]+e[1]*e[9]*e[6]+e[1]*e[0]*e[15]+e[1]*e[11]*e[8]+e[1]*e[2]*e[17]+1.500000000*e[16]*ep2[7]+.5000000000*e[16]*ep2[6]+.5000000000*e[16]*ep2[8]+.5000000000*ep2[1]*e[16]-.5000000000*e[16]*ep2[0]-.5000000000*e[16]*ep2[5]-.5000000000*e[16]*ep2[3]-.5000000000*e[16]*ep2[2]+.5000000000*ep2[4]*e[16];
+        A[33]=e[0]*e[30]*e[21]-1.*e[0]*e[35]*e[26]-1.*e[0]*e[31]*e[22]-1.*e[0]*e[32]*e[23]-1.*e[0]*e[34]*e[25]-1.*e[18]*e[34]*e[7]-1.*e[18]*e[32]*e[5]-1.*e[18]*e[35]*e[8]-1.*e[18]*e[31]*e[4]-1.*e[27]*e[26]*e[8]-1.*e[27]*e[22]*e[4]-1.*e[27]*e[25]*e[7]-1.*e[27]*e[23]*e[5]+e[6]*e[28]*e[25]+e[6]*e[19]*e[34]+e[6]*e[29]*e[26]+e[6]*e[20]*e[35]+e[21]*e[28]*e[4]+e[21]*e[1]*e[31]+e[21]*e[29]*e[5]+e[21]*e[2]*e[32]+e[30]*e[19]*e[4]+e[30]*e[1]*e[22]+e[30]*e[20]*e[5]+e[30]*e[2]*e[23]+e[24]*e[27]*e[6]+e[24]*e[0]*e[33]+e[24]*e[28]*e[7]+e[24]*e[1]*e[34]+e[24]*e[29]*e[8]+e[24]*e[2]*e[35]+e[33]*e[18]*e[6]+e[33]*e[19]*e[7]+e[33]*e[1]*e[25]+e[33]*e[20]*e[8]+e[33]*e[2]*e[26]+3.*e[0]*e[27]*e[18]+e[0]*e[28]*e[19]+e[0]*e[29]*e[20]+e[18]*e[28]*e[1]+e[18]*e[29]*e[2]+e[27]*e[19]*e[1]+e[27]*e[20]*e[2]+e[3]*e[27]*e[21]+e[3]*e[18]*e[30]+e[3]*e[28]*e[22]+e[3]*e[19]*e[31]+e[3]*e[29]*e[23]+e[3]*e[20]*e[32];
+        A[179]=e[19]*e[18]*e[6]+e[19]*e[0]*e[24]+e[19]*e[1]*e[25]+e[19]*e[20]*e[8]+e[19]*e[2]*e[26]+e[22]*e[21]*e[6]+e[22]*e[3]*e[24]+e[22]*e[4]*e[25]+e[22]*e[23]*e[8]+e[22]*e[5]*e[26]-1.*e[25]*e[21]*e[3]+e[25]*e[26]*e[8]-1.*e[25]*e[20]*e[2]-1.*e[25]*e[18]*e[0]-1.*e[25]*e[23]*e[5]+e[25]*e[24]*e[6]+e[1]*e[18]*e[24]+e[1]*e[20]*e[26]+e[4]*e[21]*e[24]+e[4]*e[23]*e[26]+.5000000000*ep2[19]*e[7]+.5000000000*ep2[22]*e[7]+1.500000000*ep2[25]*e[7]+.5000000000*e[7]*ep2[26]-.5000000000*e[7]*ep2[18]-.5000000000*e[7]*ep2[23]-.5000000000*e[7]*ep2[20]+.5000000000*e[7]*ep2[24]-.5000000000*e[7]*ep2[21];
+        A[34]=.5000000000*e[18]*ep2[11]+1.500000000*e[18]*ep2[9]+.5000000000*e[18]*ep2[10]+.5000000000*e[18]*ep2[12]+.5000000000*e[18]*ep2[15]-.5000000000*e[18]*ep2[16]-.5000000000*e[18]*ep2[17]-.5000000000*e[18]*ep2[14]-.5000000000*e[18]*ep2[13]+e[12]*e[9]*e[21]+e[12]*e[20]*e[14]+e[12]*e[11]*e[23]+e[12]*e[19]*e[13]+e[12]*e[10]*e[22]+e[21]*e[11]*e[14]+e[21]*e[10]*e[13]+e[15]*e[9]*e[24]+e[15]*e[20]*e[17]+e[15]*e[11]*e[26]+e[15]*e[19]*e[16]+e[15]*e[10]*e[25]+e[24]*e[11]*e[17]+e[24]*e[10]*e[16]-1.*e[9]*e[23]*e[14]-1.*e[9]*e[25]*e[16]-1.*e[9]*e[26]*e[17]+e[9]*e[20]*e[11]+e[9]*e[19]*e[10]-1.*e[9]*e[22]*e[13];
+        A[176]=e[13]*e[21]*e[24]+e[13]*e[23]*e[26]+e[19]*e[18]*e[15]+e[19]*e[9]*e[24]+e[19]*e[20]*e[17]+e[19]*e[11]*e[26]-1.*e[25]*e[23]*e[14]-1.*e[25]*e[20]*e[11]-1.*e[25]*e[18]*e[9]-1.*e[25]*e[21]*e[12]+e[22]*e[21]*e[15]+e[22]*e[12]*e[24]+e[22]*e[23]*e[17]+e[22]*e[14]*e[26]+e[22]*e[13]*e[25]+e[25]*e[24]*e[15]+e[25]*e[26]*e[17]+e[10]*e[19]*e[25]+e[10]*e[18]*e[24]+e[10]*e[20]*e[26]-.5000000000*e[16]*ep2[18]-.5000000000*e[16]*ep2[23]+.5000000000*e[16]*ep2[19]-.5000000000*e[16]*ep2[20]-.5000000000*e[16]*ep2[21]+.5000000000*ep2[22]*e[16]+1.500000000*ep2[25]*e[16]+.5000000000*e[16]*ep2[24]+.5000000000*e[16]*ep2[26];
+        A[35]=.5000000000*e[0]*ep2[12]+.5000000000*e[0]*ep2[15]+.5000000000*e[0]*ep2[11]+1.500000000*e[0]*ep2[9]+.5000000000*e[0]*ep2[10]-.5000000000*e[0]*ep2[16]-.5000000000*e[0]*ep2[17]-.5000000000*e[0]*ep2[14]-.5000000000*e[0]*ep2[13]+e[12]*e[9]*e[3]+e[12]*e[10]*e[4]+e[12]*e[1]*e[13]+e[12]*e[11]*e[5]+e[12]*e[2]*e[14]+e[15]*e[9]*e[6]+e[15]*e[10]*e[7]+e[15]*e[1]*e[16]+e[15]*e[11]*e[8]+e[15]*e[2]*e[17]+e[6]*e[11]*e[17]+e[6]*e[10]*e[16]+e[3]*e[11]*e[14]+e[3]*e[10]*e[13]+e[9]*e[10]*e[1]+e[9]*e[11]*e[2]-1.*e[9]*e[13]*e[4]-1.*e[9]*e[16]*e[7]-1.*e[9]*e[14]*e[5]-1.*e[9]*e[17]*e[8];
+        A[177]=e[19]*e[11]*e[35]+e[28]*e[18]*e[15]+e[28]*e[9]*e[24]+e[28]*e[20]*e[17]+e[28]*e[11]*e[26]-1.*e[25]*e[27]*e[9]-1.*e[25]*e[30]*e[12]-1.*e[25]*e[32]*e[14]+e[25]*e[33]*e[15]+e[25]*e[35]*e[17]-1.*e[25]*e[29]*e[11]-1.*e[34]*e[23]*e[14]+e[34]*e[24]*e[15]+e[34]*e[26]*e[17]-1.*e[34]*e[20]*e[11]-1.*e[34]*e[18]*e[9]-1.*e[34]*e[21]*e[12]+e[13]*e[30]*e[24]+e[13]*e[21]*e[33]+e[13]*e[31]*e[25]+e[13]*e[22]*e[34]+e[13]*e[32]*e[26]+e[13]*e[23]*e[35]+e[10]*e[27]*e[24]+e[10]*e[18]*e[33]+e[10]*e[28]*e[25]+e[10]*e[19]*e[34]+e[10]*e[29]*e[26]+e[10]*e[20]*e[35]+e[22]*e[30]*e[15]+e[22]*e[12]*e[33]+e[22]*e[32]*e[17]+e[22]*e[14]*e[35]+e[22]*e[31]*e[16]+e[31]*e[21]*e[15]+e[31]*e[12]*e[24]+e[31]*e[23]*e[17]+e[31]*e[14]*e[26]-1.*e[16]*e[27]*e[18]+e[16]*e[33]*e[24]-1.*e[16]*e[30]*e[21]-1.*e[16]*e[29]*e[20]+e[16]*e[35]*e[26]-1.*e[16]*e[32]*e[23]+e[16]*e[28]*e[19]+3.*e[16]*e[34]*e[25]+e[19]*e[27]*e[15]+e[19]*e[9]*e[33]+e[19]*e[29]*e[17];
+        A[45]=e[4]*e[27]*e[3]+e[4]*e[0]*e[30]+e[4]*e[29]*e[5]+e[4]*e[2]*e[32]+e[31]*e[0]*e[3]+e[31]*e[2]*e[5]+e[7]*e[27]*e[6]+e[7]*e[0]*e[33]+e[7]*e[29]*e[8]+e[7]*e[2]*e[35]+e[34]*e[0]*e[6]+e[34]*e[2]*e[8]+e[1]*e[27]*e[0]+e[1]*e[29]*e[2]+e[1]*e[34]*e[7]-1.*e[1]*e[32]*e[5]-1.*e[1]*e[33]*e[6]-1.*e[1]*e[30]*e[3]-1.*e[1]*e[35]*e[8]+e[1]*e[31]*e[4]+1.500000000*e[28]*ep2[1]+.5000000000*e[28]*ep2[4]+.5000000000*e[28]*ep2[0]-.5000000000*e[28]*ep2[6]-.5000000000*e[28]*ep2[5]+.5000000000*e[28]*ep2[7]-.5000000000*e[28]*ep2[3]+.5000000000*e[28]*ep2[2]-.5000000000*e[28]*ep2[8];
+        A[191]=-1.*e[35]*e[10]*e[1]-1.*e[35]*e[13]*e[4]+e[35]*e[16]*e[7]+e[35]*e[15]*e[6]-1.*e[35]*e[9]*e[0]-1.*e[35]*e[12]*e[3]+e[32]*e[12]*e[6]+e[32]*e[3]*e[15]+e[32]*e[13]*e[7]+e[32]*e[4]*e[16]-1.*e[8]*e[27]*e[9]-1.*e[8]*e[30]*e[12]-1.*e[8]*e[28]*e[10]-1.*e[8]*e[31]*e[13]+e[8]*e[29]*e[11]+e[11]*e[27]*e[6]+e[11]*e[0]*e[33]+e[11]*e[28]*e[7]+e[11]*e[1]*e[34]+e[29]*e[9]*e[6]+e[29]*e[0]*e[15]+e[29]*e[10]*e[7]+e[29]*e[1]*e[16]+e[5]*e[30]*e[15]+e[5]*e[12]*e[33]+e[5]*e[32]*e[17]+e[5]*e[14]*e[35]+e[5]*e[31]*e[16]+e[5]*e[13]*e[34]+e[8]*e[33]*e[15]+3.*e[8]*e[35]*e[17]+e[8]*e[34]*e[16]+e[2]*e[27]*e[15]+e[2]*e[9]*e[33]+e[2]*e[29]*e[17]+e[2]*e[11]*e[35]+e[2]*e[28]*e[16]+e[2]*e[10]*e[34]-1.*e[17]*e[27]*e[0]+e[17]*e[34]*e[7]+e[17]*e[33]*e[6]-1.*e[17]*e[30]*e[3]-1.*e[17]*e[28]*e[1]-1.*e[17]*e[31]*e[4]+e[14]*e[30]*e[6]+e[14]*e[3]*e[33]+e[14]*e[31]*e[7]+e[14]*e[4]*e[34]+e[14]*e[32]*e[8];
+        A[44]=e[19]*e[11]*e[2]+e[4]*e[18]*e[12]+e[4]*e[9]*e[21]+e[4]*e[20]*e[14]+e[4]*e[11]*e[23]+e[4]*e[19]*e[13]+e[4]*e[10]*e[22]+e[7]*e[18]*e[15]+e[7]*e[9]*e[24]+e[7]*e[20]*e[17]+e[7]*e[11]*e[26]+e[7]*e[19]*e[16]+e[7]*e[10]*e[25]+e[1]*e[18]*e[9]+e[1]*e[20]*e[11]-1.*e[10]*e[21]*e[3]-1.*e[10]*e[26]*e[8]-1.*e[10]*e[23]*e[5]-1.*e[10]*e[24]*e[6]+e[13]*e[18]*e[3]+e[13]*e[0]*e[21]+e[13]*e[1]*e[22]+e[13]*e[20]*e[5]+e[13]*e[2]*e[23]-1.*e[19]*e[15]*e[6]-1.*e[19]*e[14]*e[5]-1.*e[19]*e[12]*e[3]-1.*e[19]*e[17]*e[8]+e[22]*e[9]*e[3]+e[22]*e[0]*e[12]+e[22]*e[11]*e[5]+e[22]*e[2]*e[14]+e[16]*e[18]*e[6]+e[16]*e[0]*e[24]+e[16]*e[1]*e[25]+e[16]*e[20]*e[8]+e[16]*e[2]*e[26]-1.*e[1]*e[23]*e[14]-1.*e[1]*e[24]*e[15]-1.*e[1]*e[26]*e[17]-1.*e[1]*e[21]*e[12]+e[25]*e[9]*e[6]+e[25]*e[0]*e[15]+e[25]*e[11]*e[8]+e[25]*e[2]*e[17]+e[10]*e[18]*e[0]+3.*e[10]*e[19]*e[1]+e[10]*e[20]*e[2]+e[19]*e[9]*e[0];
+        A[190]=.5000000000*ep2[23]*e[26]+.5000000000*e[26]*ep2[25]+.5000000000*ep2[20]*e[26]-.5000000000*e[26]*ep2[18]+.5000000000*ep3[26]+.5000000000*e[26]*ep2[24]+e[20]*e[19]*e[25]-.5000000000*e[26]*ep2[19]-.5000000000*e[26]*ep2[21]+e[20]*e[18]*e[24]-.5000000000*e[26]*ep2[22]+e[23]*e[21]*e[24]+e[23]*e[22]*e[25];
+        A[47]=e[16]*e[9]*e[33]+e[16]*e[29]*e[17]+e[16]*e[11]*e[35]+e[16]*e[10]*e[34]+e[34]*e[11]*e[17]+e[34]*e[9]*e[15]-1.*e[10]*e[30]*e[12]-1.*e[10]*e[32]*e[14]-1.*e[10]*e[33]*e[15]-1.*e[10]*e[35]*e[17]+e[10]*e[27]*e[9]+e[10]*e[29]*e[11]+e[13]*e[27]*e[12]+e[13]*e[9]*e[30]+e[13]*e[29]*e[14]+e[13]*e[11]*e[32]+e[13]*e[10]*e[31]+e[31]*e[11]*e[14]+e[31]*e[9]*e[12]+e[16]*e[27]*e[15]+1.500000000*e[28]*ep2[10]+.5000000000*e[28]*ep2[16]+.5000000000*e[28]*ep2[9]+.5000000000*e[28]*ep2[11]-.5000000000*e[28]*ep2[12]-.5000000000*e[28]*ep2[15]-.5000000000*e[28]*ep2[17]-.5000000000*e[28]*ep2[14]+.5000000000*e[28]*ep2[13];
+        A[189]=.5000000000*ep2[20]*e[35]+.5000000000*ep2[23]*e[35]+1.500000000*e[35]*ep2[26]+.5000000000*e[35]*ep2[25]+.5000000000*e[35]*ep2[24]-.5000000000*e[35]*ep2[18]-.5000000000*e[35]*ep2[19]-.5000000000*e[35]*ep2[22]-.5000000000*e[35]*ep2[21]+e[20]*e[27]*e[24]+e[20]*e[18]*e[33]+e[20]*e[28]*e[25]+e[20]*e[19]*e[34]+e[20]*e[29]*e[26]+e[29]*e[19]*e[25]+e[29]*e[18]*e[24]+e[23]*e[30]*e[24]+e[23]*e[21]*e[33]+e[23]*e[31]*e[25]+e[23]*e[22]*e[34]+e[23]*e[32]*e[26]+e[32]*e[22]*e[25]+e[32]*e[21]*e[24]+e[26]*e[33]*e[24]+e[26]*e[34]*e[25]-1.*e[26]*e[27]*e[18]-1.*e[26]*e[30]*e[21]-1.*e[26]*e[31]*e[22]-1.*e[26]*e[28]*e[19];
+        A[46]=e[4]*e[2]*e[5]+.5000000000*e[1]*ep2[0]-.5000000000*e[1]*ep2[6]+e[7]*e[0]*e[6]+.5000000000*e[1]*ep2[7]+.5000000000*e[1]*ep2[4]-.5000000000*e[1]*ep2[8]+.5000000000*e[1]*ep2[2]-.5000000000*e[1]*ep2[3]+.5000000000*ep3[1]+e[7]*e[2]*e[8]-.5000000000*e[1]*ep2[5]+e[4]*e[0]*e[3];
+        A[188]=-.5000000000*e[17]*ep2[13]-.5000000000*e[17]*ep2[9]+.5000000000*e[17]*ep2[16]+.5000000000*e[17]*ep2[15]+.5000000000*ep3[17]-.5000000000*e[17]*ep2[10]+e[14]*e[13]*e[16]+e[14]*e[12]*e[15]+.5000000000*ep2[14]*e[17]+e[11]*e[10]*e[16]-.5000000000*e[17]*ep2[12]+.5000000000*ep2[11]*e[17]+e[11]*e[9]*e[15];
+        A[41]=e[4]*e[27]*e[30]+e[4]*e[29]*e[32]+e[4]*e[28]*e[31]+e[31]*e[27]*e[3]+e[31]*e[0]*e[30]+e[31]*e[29]*e[5]+e[31]*e[2]*e[32]+e[7]*e[27]*e[33]+e[7]*e[29]*e[35]+e[7]*e[28]*e[34]+e[28]*e[27]*e[0]+e[28]*e[29]*e[2]+e[34]*e[27]*e[6]+e[34]*e[0]*e[33]+e[34]*e[29]*e[8]+e[34]*e[2]*e[35]-1.*e[28]*e[32]*e[5]-1.*e[28]*e[33]*e[6]-1.*e[28]*e[30]*e[3]-1.*e[28]*e[35]*e[8]+.5000000000*e[1]*ep2[27]+.5000000000*e[1]*ep2[29]+1.500000000*e[1]*ep2[28]+.5000000000*e[1]*ep2[31]-.5000000000*e[1]*ep2[32]-.5000000000*e[1]*ep2[33]-.5000000000*e[1]*ep2[30]+.5000000000*e[1]*ep2[34]-.5000000000*e[1]*ep2[35];
+        A[187]=.5000000000*ep2[11]*e[35]+.5000000000*e[35]*ep2[16]-.5000000000*e[35]*ep2[9]-.5000000000*e[35]*ep2[12]+.5000000000*e[35]*ep2[15]+1.500000000*e[35]*ep2[17]-.5000000000*e[35]*ep2[10]+.5000000000*e[35]*ep2[14]-.5000000000*e[35]*ep2[13]+e[11]*e[27]*e[15]+e[11]*e[9]*e[33]+e[11]*e[29]*e[17]+e[11]*e[28]*e[16]+e[11]*e[10]*e[34]+e[29]*e[9]*e[15]+e[29]*e[10]*e[16]+e[14]*e[30]*e[15]+e[14]*e[12]*e[33]+e[14]*e[32]*e[17]+e[14]*e[31]*e[16]+e[14]*e[13]*e[34]+e[32]*e[12]*e[15]+e[32]*e[13]*e[16]+e[17]*e[33]*e[15]+e[17]*e[34]*e[16]-1.*e[17]*e[27]*e[9]-1.*e[17]*e[30]*e[12]-1.*e[17]*e[28]*e[10]-1.*e[17]*e[31]*e[13];
+        A[40]=e[34]*e[27]*e[33]+e[34]*e[29]*e[35]-.5000000000*e[28]*ep2[30]-.5000000000*e[28]*ep2[35]+.5000000000*ep3[28]+.5000000000*e[28]*ep2[27]+.5000000000*e[28]*ep2[29]+e[31]*e[27]*e[30]+e[31]*e[29]*e[32]-.5000000000*e[28]*ep2[32]-.5000000000*e[28]*ep2[33]+.5000000000*e[28]*ep2[31]+.5000000000*e[28]*ep2[34];
+        A[186]=.5000000000*ep2[5]*e[8]+e[2]*e[0]*e[6]+.5000000000*ep2[2]*e[8]+.5000000000*ep3[8]-.5000000000*e[8]*ep2[0]+e[5]*e[4]*e[7]+e[5]*e[3]*e[6]+.5000000000*e[8]*ep2[7]+e[2]*e[1]*e[7]-.5000000000*e[8]*ep2[1]-.5000000000*e[8]*ep2[4]-.5000000000*e[8]*ep2[3]+.5000000000*e[8]*ep2[6];
+        A[43]=e[28]*e[27]*e[9]+e[28]*e[29]*e[11]-1.*e[28]*e[30]*e[12]+e[28]*e[31]*e[13]-1.*e[28]*e[32]*e[14]-1.*e[28]*e[33]*e[15]-1.*e[28]*e[35]*e[17]+e[31]*e[27]*e[12]+e[31]*e[9]*e[30]+e[31]*e[29]*e[14]+e[31]*e[11]*e[32]+e[13]*e[27]*e[30]+e[13]*e[29]*e[32]+e[16]*e[27]*e[33]+e[16]*e[29]*e[35]+e[34]*e[27]*e[15]+e[34]*e[9]*e[33]+e[34]*e[29]*e[17]+e[34]*e[11]*e[35]+e[34]*e[28]*e[16]+.5000000000*e[10]*ep2[27]+.5000000000*e[10]*ep2[29]+1.500000000*e[10]*ep2[28]-.5000000000*e[10]*ep2[32]+.5000000000*e[10]*ep2[31]-.5000000000*e[10]*ep2[33]-.5000000000*e[10]*ep2[30]+.5000000000*e[10]*ep2[34]-.5000000000*e[10]*ep2[35];
+        A[185]=-.5000000000*e[35]*ep2[1]+.5000000000*e[35]*ep2[7]-.5000000000*e[35]*ep2[3]+.5000000000*ep2[2]*e[35]+1.500000000*e[35]*ep2[8]-.5000000000*e[35]*ep2[4]-.5000000000*e[35]*ep2[0]+.5000000000*e[35]*ep2[6]+.5000000000*e[35]*ep2[5]+e[2]*e[27]*e[6]+e[2]*e[0]*e[33]+e[2]*e[28]*e[7]+e[2]*e[1]*e[34]+e[2]*e[29]*e[8]-1.*e[8]*e[27]*e[0]+e[8]*e[34]*e[7]+e[8]*e[32]*e[5]+e[8]*e[33]*e[6]-1.*e[8]*e[30]*e[3]-1.*e[8]*e[28]*e[1]-1.*e[8]*e[31]*e[4]+e[29]*e[1]*e[7]+e[29]*e[0]*e[6]+e[5]*e[30]*e[6]+e[5]*e[3]*e[33]+e[5]*e[31]*e[7]+e[5]*e[4]*e[34]+e[32]*e[4]*e[7]+e[32]*e[3]*e[6];
+        A[42]=e[28]*e[27]*e[18]+e[28]*e[29]*e[20]+e[22]*e[27]*e[30]+e[22]*e[29]*e[32]+e[22]*e[28]*e[31]+e[31]*e[27]*e[21]+e[31]*e[18]*e[30]+e[31]*e[29]*e[23]+e[31]*e[20]*e[32]+e[25]*e[27]*e[33]+e[25]*e[29]*e[35]+e[25]*e[28]*e[34]+e[34]*e[27]*e[24]+e[34]*e[18]*e[33]+e[34]*e[29]*e[26]+e[34]*e[20]*e[35]-1.*e[28]*e[33]*e[24]-1.*e[28]*e[30]*e[21]-1.*e[28]*e[35]*e[26]-1.*e[28]*e[32]*e[23]-.5000000000*e[19]*ep2[33]-.5000000000*e[19]*ep2[30]-.5000000000*e[19]*ep2[35]+.5000000000*e[19]*ep2[27]+.5000000000*e[19]*ep2[29]+1.500000000*e[19]*ep2[28]+.5000000000*e[19]*ep2[31]+.5000000000*e[19]*ep2[34]-.5000000000*e[19]*ep2[32];
+        A[184]=e[23]*e[3]*e[15]-1.*e[17]*e[19]*e[1]-1.*e[17]*e[22]*e[4]-1.*e[17]*e[18]*e[0]+e[17]*e[25]*e[7]+e[17]*e[24]*e[6]+e[14]*e[21]*e[6]+e[14]*e[3]*e[24]+e[14]*e[22]*e[7]+e[14]*e[4]*e[25]+e[14]*e[23]*e[8]-1.*e[26]*e[10]*e[1]-1.*e[26]*e[13]*e[4]+e[26]*e[16]*e[7]+e[26]*e[15]*e[6]-1.*e[26]*e[9]*e[0]-1.*e[26]*e[12]*e[3]+e[23]*e[12]*e[6]+e[11]*e[18]*e[6]+e[11]*e[0]*e[24]+e[11]*e[19]*e[7]+e[11]*e[1]*e[25]+e[11]*e[20]*e[8]+e[11]*e[2]*e[26]+e[20]*e[9]*e[6]+e[20]*e[0]*e[15]+e[20]*e[10]*e[7]+e[20]*e[1]*e[16]+e[20]*e[2]*e[17]+e[5]*e[21]*e[15]+e[5]*e[12]*e[24]+e[5]*e[23]*e[17]+e[5]*e[14]*e[26]+e[5]*e[22]*e[16]+e[5]*e[13]*e[25]+e[8]*e[24]*e[15]+3.*e[8]*e[26]*e[17]+e[8]*e[25]*e[16]+e[2]*e[18]*e[15]+e[2]*e[9]*e[24]+e[2]*e[19]*e[16]+e[2]*e[10]*e[25]-1.*e[17]*e[21]*e[3]+e[23]*e[4]*e[16]+e[23]*e[13]*e[7]-1.*e[8]*e[18]*e[9]-1.*e[8]*e[21]*e[12]-1.*e[8]*e[19]*e[10]-1.*e[8]*e[22]*e[13];
+        A[54]=e[13]*e[18]*e[12]+e[13]*e[9]*e[21]+e[13]*e[20]*e[14]+e[13]*e[11]*e[23]+e[13]*e[10]*e[22]+e[22]*e[11]*e[14]+e[22]*e[9]*e[12]+e[16]*e[18]*e[15]+e[16]*e[9]*e[24]+e[16]*e[20]*e[17]+e[16]*e[11]*e[26]+e[16]*e[10]*e[25]+e[25]*e[11]*e[17]+e[25]*e[9]*e[15]-1.*e[10]*e[23]*e[14]-1.*e[10]*e[24]*e[15]-1.*e[10]*e[26]*e[17]+e[10]*e[20]*e[11]+e[10]*e[18]*e[9]-1.*e[10]*e[21]*e[12]+.5000000000*e[19]*ep2[11]+.5000000000*e[19]*ep2[9]+1.500000000*e[19]*ep2[10]+.5000000000*e[19]*ep2[13]+.5000000000*e[19]*ep2[16]-.5000000000*e[19]*ep2[12]-.5000000000*e[19]*ep2[15]-.5000000000*e[19]*ep2[17]-.5000000000*e[19]*ep2[14];
+        A[164]=e[10]*e[18]*e[6]+e[10]*e[0]*e[24]+e[10]*e[19]*e[7]+e[10]*e[1]*e[25]+e[10]*e[20]*e[8]+e[10]*e[2]*e[26]+e[19]*e[9]*e[6]+e[19]*e[0]*e[15]+e[19]*e[1]*e[16]+e[19]*e[11]*e[8]+e[19]*e[2]*e[17]+e[4]*e[21]*e[15]+e[4]*e[12]*e[24]+e[4]*e[23]*e[17]+e[4]*e[14]*e[26]+e[4]*e[22]*e[16]+e[4]*e[13]*e[25]+e[7]*e[24]*e[15]+e[7]*e[26]*e[17]+3.*e[7]*e[25]*e[16]+e[1]*e[18]*e[15]+e[1]*e[9]*e[24]+e[1]*e[20]*e[17]+e[1]*e[11]*e[26]-1.*e[16]*e[21]*e[3]+e[16]*e[26]*e[8]-1.*e[16]*e[20]*e[2]-1.*e[16]*e[18]*e[0]-1.*e[16]*e[23]*e[5]+e[16]*e[24]*e[6]+e[13]*e[21]*e[6]+e[13]*e[3]*e[24]+e[13]*e[22]*e[7]+e[13]*e[23]*e[8]+e[13]*e[5]*e[26]-1.*e[25]*e[11]*e[2]+e[25]*e[15]*e[6]-1.*e[25]*e[9]*e[0]-1.*e[25]*e[14]*e[5]-1.*e[25]*e[12]*e[3]+e[25]*e[17]*e[8]+e[22]*e[12]*e[6]+e[22]*e[3]*e[15]+e[22]*e[14]*e[8]+e[22]*e[5]*e[17]-1.*e[7]*e[23]*e[14]-1.*e[7]*e[20]*e[11]-1.*e[7]*e[18]*e[9]-1.*e[7]*e[21]*e[12];
+        A[55]=e[13]*e[9]*e[3]+e[13]*e[0]*e[12]+e[13]*e[10]*e[4]+e[13]*e[11]*e[5]+e[13]*e[2]*e[14]+e[16]*e[9]*e[6]+e[16]*e[0]*e[15]+e[16]*e[10]*e[7]+e[16]*e[11]*e[8]+e[16]*e[2]*e[17]+e[7]*e[11]*e[17]+e[7]*e[9]*e[15]+e[4]*e[11]*e[14]+e[4]*e[9]*e[12]+e[10]*e[9]*e[0]+e[10]*e[11]*e[2]-1.*e[10]*e[15]*e[6]-1.*e[10]*e[14]*e[5]-1.*e[10]*e[12]*e[3]-1.*e[10]*e[17]*e[8]+.5000000000*e[1]*ep2[11]+.5000000000*e[1]*ep2[9]+1.500000000*e[1]*ep2[10]-.5000000000*e[1]*ep2[12]-.5000000000*e[1]*ep2[15]-.5000000000*e[1]*ep2[17]-.5000000000*e[1]*ep2[14]+.5000000000*e[1]*ep2[13]+.5000000000*e[1]*ep2[16];
+        A[165]=e[1]*e[27]*e[6]+e[1]*e[0]*e[33]+e[1]*e[28]*e[7]+e[1]*e[29]*e[8]+e[1]*e[2]*e[35]-1.*e[7]*e[27]*e[0]-1.*e[7]*e[32]*e[5]+e[7]*e[33]*e[6]-1.*e[7]*e[30]*e[3]+e[7]*e[35]*e[8]-1.*e[7]*e[29]*e[2]+e[7]*e[31]*e[4]+e[28]*e[0]*e[6]+e[28]*e[2]*e[8]+e[4]*e[30]*e[6]+e[4]*e[3]*e[33]+e[4]*e[32]*e[8]+e[4]*e[5]*e[35]+e[31]*e[3]*e[6]+e[31]*e[5]*e[8]+.5000000000*ep2[1]*e[34]+1.500000000*e[34]*ep2[7]+.5000000000*e[34]*ep2[4]-.5000000000*e[34]*ep2[0]+.5000000000*e[34]*ep2[6]-.5000000000*e[34]*ep2[5]-.5000000000*e[34]*ep2[3]-.5000000000*e[34]*ep2[2]+.5000000000*e[34]*ep2[8];
+        A[52]=e[4]*e[18]*e[3]+e[4]*e[0]*e[21]+e[4]*e[1]*e[22]+e[4]*e[20]*e[5]+e[4]*e[2]*e[23]+e[22]*e[0]*e[3]+e[22]*e[2]*e[5]+e[7]*e[18]*e[6]+e[7]*e[0]*e[24]+e[7]*e[1]*e[25]+e[7]*e[20]*e[8]+e[7]*e[2]*e[26]+e[25]*e[0]*e[6]+e[25]*e[2]*e[8]+e[1]*e[18]*e[0]+e[1]*e[20]*e[2]-1.*e[1]*e[21]*e[3]-1.*e[1]*e[26]*e[8]-1.*e[1]*e[23]*e[5]-1.*e[1]*e[24]*e[6]+.5000000000*e[19]*ep2[4]+.5000000000*e[19]*ep2[0]-.5000000000*e[19]*ep2[6]-.5000000000*e[19]*ep2[5]+1.500000000*e[19]*ep2[1]+.5000000000*e[19]*ep2[7]-.5000000000*e[19]*ep2[3]+.5000000000*e[19]*ep2[2]-.5000000000*e[19]*ep2[8];
+        A[166]=-.5000000000*e[7]*ep2[0]+e[4]*e[5]*e[8]+.5000000000*ep2[4]*e[7]-.5000000000*e[7]*ep2[2]+.5000000000*e[7]*ep2[8]-.5000000000*e[7]*ep2[5]+.5000000000*e[7]*ep2[6]+e[1]*e[0]*e[6]+.5000000000*ep3[7]+e[4]*e[3]*e[6]+e[1]*e[2]*e[8]-.5000000000*e[7]*ep2[3]+.5000000000*ep2[1]*e[7];
+        A[53]=-1.*e[1]*e[32]*e[23]-1.*e[19]*e[32]*e[5]-1.*e[19]*e[33]*e[6]-1.*e[19]*e[30]*e[3]-1.*e[19]*e[35]*e[8]-1.*e[28]*e[21]*e[3]-1.*e[28]*e[26]*e[8]-1.*e[28]*e[23]*e[5]-1.*e[28]*e[24]*e[6]+e[7]*e[27]*e[24]+e[7]*e[18]*e[33]+e[7]*e[29]*e[26]+e[7]*e[20]*e[35]+e[22]*e[27]*e[3]+e[22]*e[0]*e[30]+e[22]*e[29]*e[5]+e[22]*e[2]*e[32]+e[31]*e[18]*e[3]+e[31]*e[0]*e[21]+e[31]*e[20]*e[5]+e[31]*e[2]*e[23]+e[25]*e[27]*e[6]+e[25]*e[0]*e[33]+e[25]*e[28]*e[7]+e[25]*e[1]*e[34]+e[25]*e[29]*e[8]+e[25]*e[2]*e[35]+e[34]*e[18]*e[6]+e[34]*e[0]*e[24]+e[34]*e[19]*e[7]+e[34]*e[20]*e[8]+e[34]*e[2]*e[26]+e[1]*e[27]*e[18]+3.*e[1]*e[28]*e[19]+e[1]*e[29]*e[20]+e[19]*e[27]*e[0]+e[19]*e[29]*e[2]+e[28]*e[18]*e[0]+e[28]*e[20]*e[2]+e[4]*e[27]*e[21]+e[4]*e[18]*e[30]+e[4]*e[28]*e[22]+e[4]*e[19]*e[31]+e[4]*e[29]*e[23]+e[4]*e[20]*e[32]-1.*e[1]*e[33]*e[24]-1.*e[1]*e[30]*e[21]-1.*e[1]*e[35]*e[26]+e[1]*e[31]*e[22];
+        A[167]=e[10]*e[27]*e[15]+e[10]*e[9]*e[33]+e[10]*e[29]*e[17]+e[10]*e[11]*e[35]+e[10]*e[28]*e[16]+e[28]*e[11]*e[17]+e[28]*e[9]*e[15]+e[13]*e[30]*e[15]+e[13]*e[12]*e[33]+e[13]*e[32]*e[17]+e[13]*e[14]*e[35]+e[13]*e[31]*e[16]+e[31]*e[14]*e[17]+e[31]*e[12]*e[15]+e[16]*e[33]*e[15]+e[16]*e[35]*e[17]-1.*e[16]*e[27]*e[9]-1.*e[16]*e[30]*e[12]-1.*e[16]*e[32]*e[14]-1.*e[16]*e[29]*e[11]+.5000000000*ep2[10]*e[34]+1.500000000*e[34]*ep2[16]-.5000000000*e[34]*ep2[9]-.5000000000*e[34]*ep2[11]-.5000000000*e[34]*ep2[12]+.5000000000*e[34]*ep2[15]+.5000000000*e[34]*ep2[17]-.5000000000*e[34]*ep2[14]+.5000000000*e[34]*ep2[13];
+        A[50]=.5000000000*e[19]*ep2[18]+.5000000000*e[19]*ep2[25]+.5000000000*e[19]*ep2[22]+e[25]*e[20]*e[26]-.5000000000*e[19]*ep2[21]+.5000000000*e[19]*ep2[20]-.5000000000*e[19]*ep2[26]-.5000000000*e[19]*ep2[23]-.5000000000*e[19]*ep2[24]+.5000000000*ep3[19]+e[22]*e[20]*e[23]+e[25]*e[18]*e[24]+e[22]*e[18]*e[21];
+        A[160]=.5000000000*e[34]*ep2[33]+.5000000000*e[34]*ep2[35]-.5000000000*e[34]*ep2[27]-.5000000000*e[34]*ep2[32]-.5000000000*e[34]*ep2[29]-.5000000000*e[34]*ep2[30]+.5000000000*ep2[28]*e[34]+e[31]*e[30]*e[33]+e[31]*e[32]*e[35]+e[28]*e[27]*e[33]+.5000000000*ep3[34]+e[28]*e[29]*e[35]+.5000000000*ep2[31]*e[34];
+        A[51]=e[4]*e[28]*e[13]+e[4]*e[10]*e[31]+e[7]*e[27]*e[15]+e[7]*e[9]*e[33]+e[7]*e[29]*e[17]+e[7]*e[11]*e[35]+e[7]*e[28]*e[16]+e[7]*e[10]*e[34]+e[1]*e[27]*e[9]+e[1]*e[29]*e[11]+3.*e[1]*e[28]*e[10]+e[10]*e[27]*e[0]-1.*e[10]*e[32]*e[5]-1.*e[10]*e[33]*e[6]-1.*e[10]*e[30]*e[3]-1.*e[10]*e[35]*e[8]+e[10]*e[29]*e[2]+e[13]*e[27]*e[3]+e[13]*e[0]*e[30]+e[13]*e[1]*e[31]+e[13]*e[29]*e[5]+e[13]*e[2]*e[32]+e[28]*e[11]*e[2]-1.*e[28]*e[15]*e[6]+e[28]*e[9]*e[0]-1.*e[28]*e[14]*e[5]-1.*e[28]*e[12]*e[3]-1.*e[28]*e[17]*e[8]+e[31]*e[9]*e[3]+e[31]*e[0]*e[12]+e[31]*e[11]*e[5]+e[31]*e[2]*e[14]+e[16]*e[27]*e[6]+e[16]*e[0]*e[33]+e[16]*e[1]*e[34]+e[16]*e[29]*e[8]+e[16]*e[2]*e[35]-1.*e[1]*e[30]*e[12]-1.*e[1]*e[32]*e[14]-1.*e[1]*e[33]*e[15]-1.*e[1]*e[35]*e[17]+e[34]*e[9]*e[6]+e[34]*e[0]*e[15]+e[34]*e[11]*e[8]+e[34]*e[2]*e[17]+e[4]*e[27]*e[12]+e[4]*e[9]*e[30]+e[4]*e[29]*e[14]+e[4]*e[11]*e[32];
+        A[161]=e[4]*e[30]*e[33]+e[4]*e[32]*e[35]+e[4]*e[31]*e[34]+e[31]*e[30]*e[6]+e[31]*e[3]*e[33]+e[31]*e[32]*e[8]+e[31]*e[5]*e[35]+e[28]*e[27]*e[6]+e[28]*e[0]*e[33]+e[28]*e[29]*e[8]+e[28]*e[2]*e[35]+e[34]*e[33]*e[6]+e[34]*e[35]*e[8]-1.*e[34]*e[27]*e[0]-1.*e[34]*e[32]*e[5]-1.*e[34]*e[30]*e[3]-1.*e[34]*e[29]*e[2]+e[1]*e[27]*e[33]+e[1]*e[29]*e[35]+e[1]*e[28]*e[34]+.5000000000*ep2[31]*e[7]-.5000000000*e[7]*ep2[27]-.5000000000*e[7]*ep2[32]+.5000000000*e[7]*ep2[28]-.5000000000*e[7]*ep2[29]+.5000000000*e[7]*ep2[33]-.5000000000*e[7]*ep2[30]+1.500000000*e[7]*ep2[34]+.5000000000*e[7]*ep2[35];
+        A[48]=-.5000000000*e[10]*ep2[14]-.5000000000*e[10]*ep2[17]-.5000000000*e[10]*ep2[15]+e[13]*e[11]*e[14]+e[16]*e[11]*e[17]+.5000000000*e[10]*ep2[13]+e[13]*e[9]*e[12]-.5000000000*e[10]*ep2[12]+.5000000000*ep3[10]+e[16]*e[9]*e[15]+.5000000000*e[10]*ep2[16]+.5000000000*e[10]*ep2[11]+.5000000000*e[10]*ep2[9];
+        A[162]=e[22]*e[32]*e[35]+e[22]*e[31]*e[34]+e[31]*e[30]*e[24]+e[31]*e[21]*e[33]+e[31]*e[32]*e[26]+e[31]*e[23]*e[35]+e[34]*e[33]*e[24]+e[34]*e[35]*e[26]-1.*e[34]*e[27]*e[18]-1.*e[34]*e[30]*e[21]-1.*e[34]*e[29]*e[20]-1.*e[34]*e[32]*e[23]+e[19]*e[27]*e[33]+e[19]*e[29]*e[35]+e[19]*e[28]*e[34]+e[28]*e[27]*e[24]+e[28]*e[18]*e[33]+e[28]*e[29]*e[26]+e[28]*e[20]*e[35]+e[22]*e[30]*e[33]+.5000000000*ep2[28]*e[25]+.5000000000*ep2[31]*e[25]+.5000000000*e[25]*ep2[33]+.5000000000*e[25]*ep2[35]+1.500000000*e[25]*ep2[34]-.5000000000*e[25]*ep2[27]-.5000000000*e[25]*ep2[32]-.5000000000*e[25]*ep2[29]-.5000000000*e[25]*ep2[30];
+        A[49]=-1.*e[19]*e[35]*e[26]-1.*e[19]*e[32]*e[23]+e[19]*e[27]*e[18]+e[19]*e[29]*e[20]+e[22]*e[27]*e[21]+e[22]*e[18]*e[30]+e[22]*e[19]*e[31]+e[22]*e[29]*e[23]+e[22]*e[20]*e[32]+e[31]*e[18]*e[21]+e[31]*e[20]*e[23]+e[25]*e[27]*e[24]+e[25]*e[18]*e[33]+e[25]*e[19]*e[34]+e[25]*e[29]*e[26]+e[25]*e[20]*e[35]+e[34]*e[18]*e[24]+e[34]*e[20]*e[26]-1.*e[19]*e[33]*e[24]-1.*e[19]*e[30]*e[21]+1.500000000*e[28]*ep2[19]+.5000000000*e[28]*ep2[18]+.5000000000*e[28]*ep2[20]+.5000000000*e[28]*ep2[22]+.5000000000*e[28]*ep2[25]-.5000000000*e[28]*ep2[26]-.5000000000*e[28]*ep2[23]-.5000000000*e[28]*ep2[24]-.5000000000*e[28]*ep2[21];
+        A[163]=e[10]*e[27]*e[33]+e[10]*e[29]*e[35]+e[10]*e[28]*e[34]+e[34]*e[33]*e[15]+e[34]*e[35]*e[17]+e[28]*e[27]*e[15]+e[28]*e[9]*e[33]+e[28]*e[29]*e[17]+e[28]*e[11]*e[35]-1.*e[34]*e[27]*e[9]-1.*e[34]*e[30]*e[12]+e[34]*e[31]*e[13]-1.*e[34]*e[32]*e[14]-1.*e[34]*e[29]*e[11]+e[31]*e[30]*e[15]+e[31]*e[12]*e[33]+e[31]*e[32]*e[17]+e[31]*e[14]*e[35]+e[13]*e[30]*e[33]+e[13]*e[32]*e[35]-.5000000000*e[16]*ep2[27]-.5000000000*e[16]*ep2[32]+.5000000000*e[16]*ep2[28]-.5000000000*e[16]*ep2[29]+.5000000000*e[16]*ep2[31]+.5000000000*e[16]*ep2[33]-.5000000000*e[16]*ep2[30]+1.500000000*e[16]*ep2[34]+.5000000000*e[16]*ep2[35];
+        A[63]=e[29]*e[32]*e[14]-1.*e[29]*e[33]*e[15]-1.*e[29]*e[34]*e[16]+e[32]*e[27]*e[12]+e[32]*e[9]*e[30]+e[32]*e[28]*e[13]+e[32]*e[10]*e[31]+e[14]*e[27]*e[30]+e[14]*e[28]*e[31]+e[17]*e[27]*e[33]+e[17]*e[28]*e[34]+e[35]*e[27]*e[15]+e[35]*e[9]*e[33]+e[35]*e[29]*e[17]+e[35]*e[28]*e[16]+e[35]*e[10]*e[34]+e[29]*e[27]*e[9]+e[29]*e[28]*e[10]-1.*e[29]*e[30]*e[12]-1.*e[29]*e[31]*e[13]+.5000000000*e[11]*ep2[27]+1.500000000*e[11]*ep2[29]+.5000000000*e[11]*ep2[28]+.5000000000*e[11]*ep2[32]-.5000000000*e[11]*ep2[31]-.5000000000*e[11]*ep2[33]-.5000000000*e[11]*ep2[30]-.5000000000*e[11]*ep2[34]+.5000000000*e[11]*ep2[35];
+        A[173]=e[1]*e[20]*e[35]+e[19]*e[27]*e[6]+e[19]*e[0]*e[33]+e[19]*e[28]*e[7]+e[19]*e[29]*e[8]+e[19]*e[2]*e[35]+e[28]*e[18]*e[6]+e[28]*e[0]*e[24]+e[28]*e[20]*e[8]+e[28]*e[2]*e[26]+e[4]*e[30]*e[24]+e[4]*e[21]*e[33]+e[4]*e[31]*e[25]+e[4]*e[22]*e[34]+e[4]*e[32]*e[26]+e[4]*e[23]*e[35]-1.*e[7]*e[27]*e[18]+e[7]*e[33]*e[24]-1.*e[7]*e[30]*e[21]-1.*e[7]*e[29]*e[20]+e[7]*e[35]*e[26]+e[7]*e[31]*e[22]-1.*e[7]*e[32]*e[23]-1.*e[25]*e[27]*e[0]-1.*e[25]*e[32]*e[5]-1.*e[25]*e[30]*e[3]-1.*e[25]*e[29]*e[2]-1.*e[34]*e[21]*e[3]-1.*e[34]*e[20]*e[2]-1.*e[34]*e[18]*e[0]-1.*e[34]*e[23]*e[5]+e[22]*e[30]*e[6]+e[22]*e[3]*e[33]+e[22]*e[32]*e[8]+e[22]*e[5]*e[35]+e[31]*e[21]*e[6]+e[31]*e[3]*e[24]+e[31]*e[23]*e[8]+e[31]*e[5]*e[26]+e[34]*e[26]*e[8]+e[1]*e[27]*e[24]+e[1]*e[18]*e[33]+e[1]*e[28]*e[25]+e[1]*e[19]*e[34]+e[1]*e[29]*e[26]+e[34]*e[24]*e[6]+e[25]*e[33]*e[6]+3.*e[25]*e[34]*e[7]+e[25]*e[35]*e[8];
+        A[62]=.5000000000*e[20]*ep2[27]+1.500000000*e[20]*ep2[29]+.5000000000*e[20]*ep2[28]+.5000000000*e[20]*ep2[32]+.5000000000*e[20]*ep2[35]-.5000000000*e[20]*ep2[31]-.5000000000*e[20]*ep2[33]-.5000000000*e[20]*ep2[30]-.5000000000*e[20]*ep2[34]+e[29]*e[27]*e[18]+e[29]*e[28]*e[19]+e[23]*e[27]*e[30]+e[23]*e[29]*e[32]+e[23]*e[28]*e[31]+e[32]*e[27]*e[21]+e[32]*e[18]*e[30]+e[32]*e[28]*e[22]+e[32]*e[19]*e[31]+e[26]*e[27]*e[33]+e[26]*e[29]*e[35]+e[26]*e[28]*e[34]+e[35]*e[27]*e[24]+e[35]*e[18]*e[33]+e[35]*e[28]*e[25]+e[35]*e[19]*e[34]-1.*e[29]*e[33]*e[24]-1.*e[29]*e[30]*e[21]-1.*e[29]*e[31]*e[22]-1.*e[29]*e[34]*e[25];
+        A[172]=e[19]*e[1]*e[7]+e[19]*e[0]*e[6]+e[19]*e[2]*e[8]+e[4]*e[21]*e[6]+e[4]*e[3]*e[24]+e[4]*e[22]*e[7]+e[4]*e[23]*e[8]+e[4]*e[5]*e[26]+e[22]*e[3]*e[6]+e[22]*e[5]*e[8]+e[7]*e[24]*e[6]+e[7]*e[26]*e[8]+e[1]*e[18]*e[6]+e[1]*e[0]*e[24]+e[1]*e[20]*e[8]+e[1]*e[2]*e[26]-1.*e[7]*e[21]*e[3]-1.*e[7]*e[20]*e[2]-1.*e[7]*e[18]*e[0]-1.*e[7]*e[23]*e[5]+.5000000000*e[25]*ep2[4]-.5000000000*e[25]*ep2[0]+.5000000000*e[25]*ep2[6]-.5000000000*e[25]*ep2[5]+.5000000000*e[25]*ep2[1]+1.500000000*e[25]*ep2[7]-.5000000000*e[25]*ep2[3]-.5000000000*e[25]*ep2[2]+.5000000000*e[25]*ep2[8];
+        A[61]=e[5]*e[27]*e[30]+e[5]*e[29]*e[32]+e[5]*e[28]*e[31]+e[32]*e[27]*e[3]+e[32]*e[0]*e[30]+e[32]*e[28]*e[4]+e[32]*e[1]*e[31]+e[8]*e[27]*e[33]+e[8]*e[29]*e[35]+e[8]*e[28]*e[34]+e[29]*e[27]*e[0]+e[29]*e[28]*e[1]+e[35]*e[27]*e[6]+e[35]*e[0]*e[33]+e[35]*e[28]*e[7]+e[35]*e[1]*e[34]-1.*e[29]*e[34]*e[7]-1.*e[29]*e[33]*e[6]-1.*e[29]*e[30]*e[3]-1.*e[29]*e[31]*e[4]+.5000000000*e[2]*ep2[27]+1.500000000*e[2]*ep2[29]+.5000000000*e[2]*ep2[28]+.5000000000*e[2]*ep2[32]-.5000000000*e[2]*ep2[31]-.5000000000*e[2]*ep2[33]-.5000000000*e[2]*ep2[30]-.5000000000*e[2]*ep2[34]+.5000000000*e[2]*ep2[35];
+        A[175]=e[13]*e[12]*e[6]+e[13]*e[3]*e[15]+e[13]*e[4]*e[16]+e[13]*e[14]*e[8]+e[13]*e[5]*e[17]+e[16]*e[15]*e[6]+e[16]*e[17]*e[8]+e[1]*e[11]*e[17]+e[1]*e[9]*e[15]+e[1]*e[10]*e[16]+e[4]*e[14]*e[17]+e[4]*e[12]*e[15]+e[10]*e[9]*e[6]+e[10]*e[0]*e[15]+e[10]*e[11]*e[8]+e[10]*e[2]*e[17]-1.*e[16]*e[11]*e[2]-1.*e[16]*e[9]*e[0]-1.*e[16]*e[14]*e[5]-1.*e[16]*e[12]*e[3]+.5000000000*ep2[13]*e[7]+1.500000000*ep2[16]*e[7]+.5000000000*e[7]*ep2[17]+.5000000000*e[7]*ep2[15]-.5000000000*e[7]*ep2[9]-.5000000000*e[7]*ep2[11]-.5000000000*e[7]*ep2[12]+.5000000000*e[7]*ep2[10]-.5000000000*e[7]*ep2[14];
+        A[60]=.5000000000*e[29]*ep2[32]+.5000000000*e[29]*ep2[35]-.5000000000*e[29]*ep2[31]-.5000000000*e[29]*ep2[33]-.5000000000*e[29]*ep2[30]-.5000000000*e[29]*ep2[34]+e[32]*e[27]*e[30]+.5000000000*ep3[29]+.5000000000*e[29]*ep2[28]+e[35]*e[28]*e[34]+.5000000000*e[29]*ep2[27]+e[35]*e[27]*e[33]+e[32]*e[28]*e[31];
+        A[174]=-1.*e[16]*e[21]*e[12]+e[10]*e[18]*e[15]+e[10]*e[9]*e[24]+e[10]*e[20]*e[17]+e[10]*e[11]*e[26]+e[19]*e[11]*e[17]+e[19]*e[9]*e[15]+e[19]*e[10]*e[16]+e[13]*e[21]*e[15]+e[13]*e[12]*e[24]+e[13]*e[23]*e[17]+e[13]*e[14]*e[26]+e[13]*e[22]*e[16]+e[22]*e[14]*e[17]+e[22]*e[12]*e[15]+e[16]*e[24]*e[15]+e[16]*e[26]*e[17]-1.*e[16]*e[23]*e[14]-1.*e[16]*e[20]*e[11]-1.*e[16]*e[18]*e[9]+.5000000000*ep2[13]*e[25]+1.500000000*e[25]*ep2[16]+.5000000000*e[25]*ep2[17]+.5000000000*e[25]*ep2[15]+.5000000000*ep2[10]*e[25]-.5000000000*e[25]*ep2[9]-.5000000000*e[25]*ep2[11]-.5000000000*e[25]*ep2[12]-.5000000000*e[25]*ep2[14];
+        A[59]=e[19]*e[20]*e[2]+e[22]*e[18]*e[3]+e[22]*e[0]*e[21]+e[22]*e[19]*e[4]+e[22]*e[20]*e[5]+e[22]*e[2]*e[23]-1.*e[19]*e[21]*e[3]-1.*e[19]*e[26]*e[8]+e[19]*e[25]*e[7]-1.*e[19]*e[23]*e[5]-1.*e[19]*e[24]*e[6]+e[4]*e[18]*e[21]+e[4]*e[20]*e[23]+e[25]*e[18]*e[6]+e[25]*e[0]*e[24]+e[25]*e[20]*e[8]+e[25]*e[2]*e[26]+e[7]*e[18]*e[24]+e[7]*e[20]*e[26]+e[19]*e[18]*e[0]+1.500000000*ep2[19]*e[1]+.5000000000*e[1]*ep2[22]+.5000000000*e[1]*ep2[18]+.5000000000*e[1]*ep2[20]+.5000000000*e[1]*ep2[25]-.5000000000*e[1]*ep2[26]-.5000000000*e[1]*ep2[23]-.5000000000*e[1]*ep2[24]-.5000000000*e[1]*ep2[21];
+        A[169]=e[19]*e[27]*e[24]+e[19]*e[18]*e[33]+e[19]*e[28]*e[25]+e[19]*e[29]*e[26]+e[19]*e[20]*e[35]+e[28]*e[18]*e[24]+e[28]*e[20]*e[26]+e[22]*e[30]*e[24]+e[22]*e[21]*e[33]+e[22]*e[31]*e[25]+e[22]*e[32]*e[26]+e[22]*e[23]*e[35]+e[31]*e[21]*e[24]+e[31]*e[23]*e[26]+e[25]*e[33]*e[24]+e[25]*e[35]*e[26]-1.*e[25]*e[27]*e[18]-1.*e[25]*e[30]*e[21]-1.*e[25]*e[29]*e[20]-1.*e[25]*e[32]*e[23]-.5000000000*e[34]*ep2[18]-.5000000000*e[34]*ep2[23]-.5000000000*e[34]*ep2[20]-.5000000000*e[34]*ep2[21]+.5000000000*ep2[19]*e[34]+.5000000000*ep2[22]*e[34]+1.500000000*e[34]*ep2[25]+.5000000000*e[34]*ep2[24]+.5000000000*e[34]*ep2[26];
+        A[58]=e[16]*e[0]*e[6]+e[16]*e[2]*e[8]+e[1]*e[11]*e[2]-1.*e[1]*e[15]*e[6]+e[1]*e[9]*e[0]-1.*e[1]*e[14]*e[5]-1.*e[1]*e[12]*e[3]-1.*e[1]*e[17]*e[8]+e[4]*e[9]*e[3]+e[4]*e[0]*e[12]+e[4]*e[1]*e[13]+e[4]*e[11]*e[5]+e[4]*e[2]*e[14]+e[13]*e[0]*e[3]+e[13]*e[2]*e[5]+e[7]*e[9]*e[6]+e[7]*e[0]*e[15]+e[7]*e[1]*e[16]+e[7]*e[11]*e[8]+e[7]*e[2]*e[17]-.5000000000*e[10]*ep2[6]-.5000000000*e[10]*ep2[5]-.5000000000*e[10]*ep2[3]-.5000000000*e[10]*ep2[8]+1.500000000*e[10]*ep2[1]+.5000000000*e[10]*ep2[0]+.5000000000*e[10]*ep2[2]+.5000000000*e[10]*ep2[4]+.5000000000*e[10]*ep2[7];
+        A[168]=e[13]*e[14]*e[17]+e[13]*e[12]*e[15]+e[10]*e[9]*e[15]+.5000000000*e[16]*ep2[15]-.5000000000*e[16]*ep2[11]-.5000000000*e[16]*ep2[12]-.5000000000*e[16]*ep2[14]+e[10]*e[11]*e[17]+.5000000000*ep2[10]*e[16]+.5000000000*ep3[16]-.5000000000*e[16]*ep2[9]+.5000000000*e[16]*ep2[17]+.5000000000*ep2[13]*e[16];
+        A[57]=e[10]*e[29]*e[20]+e[22]*e[27]*e[12]+e[22]*e[9]*e[30]+e[22]*e[29]*e[14]+e[22]*e[11]*e[32]+e[22]*e[10]*e[31]+e[31]*e[18]*e[12]+e[31]*e[9]*e[21]+e[31]*e[20]*e[14]+e[31]*e[11]*e[23]-1.*e[10]*e[33]*e[24]-1.*e[10]*e[30]*e[21]-1.*e[10]*e[35]*e[26]-1.*e[10]*e[32]*e[23]+e[10]*e[34]*e[25]+e[19]*e[27]*e[9]+e[19]*e[29]*e[11]+e[28]*e[18]*e[9]+e[28]*e[20]*e[11]+e[16]*e[27]*e[24]+e[16]*e[18]*e[33]+e[16]*e[28]*e[25]+e[16]*e[19]*e[34]+e[16]*e[29]*e[26]+e[16]*e[20]*e[35]-1.*e[19]*e[30]*e[12]-1.*e[19]*e[32]*e[14]-1.*e[19]*e[33]*e[15]-1.*e[19]*e[35]*e[17]-1.*e[28]*e[23]*e[14]-1.*e[28]*e[24]*e[15]-1.*e[28]*e[26]*e[17]-1.*e[28]*e[21]*e[12]+e[25]*e[27]*e[15]+e[25]*e[9]*e[33]+e[25]*e[29]*e[17]+e[25]*e[11]*e[35]+e[34]*e[18]*e[15]+e[34]*e[9]*e[24]+e[34]*e[20]*e[17]+e[34]*e[11]*e[26]+e[13]*e[27]*e[21]+e[13]*e[18]*e[30]+e[13]*e[28]*e[22]+e[13]*e[19]*e[31]+e[13]*e[29]*e[23]+e[13]*e[20]*e[32]+e[10]*e[27]*e[18]+3.*e[10]*e[28]*e[19];
+        A[171]=e[4]*e[30]*e[15]+e[4]*e[12]*e[33]+e[4]*e[32]*e[17]+e[4]*e[14]*e[35]+e[4]*e[31]*e[16]+e[4]*e[13]*e[34]+e[7]*e[33]*e[15]+e[7]*e[35]*e[17]+3.*e[7]*e[34]*e[16]+e[1]*e[27]*e[15]+e[1]*e[9]*e[33]+e[1]*e[29]*e[17]+e[1]*e[11]*e[35]+e[1]*e[28]*e[16]+e[1]*e[10]*e[34]-1.*e[16]*e[27]*e[0]-1.*e[16]*e[32]*e[5]+e[16]*e[33]*e[6]-1.*e[16]*e[30]*e[3]+e[16]*e[35]*e[8]-1.*e[16]*e[29]*e[2]+e[13]*e[30]*e[6]+e[13]*e[3]*e[33]+e[13]*e[31]*e[7]+e[13]*e[32]*e[8]+e[13]*e[5]*e[35]-1.*e[34]*e[11]*e[2]+e[34]*e[15]*e[6]-1.*e[34]*e[9]*e[0]-1.*e[34]*e[14]*e[5]-1.*e[34]*e[12]*e[3]+e[34]*e[17]*e[8]+e[31]*e[12]*e[6]+e[31]*e[3]*e[15]+e[31]*e[14]*e[8]+e[31]*e[5]*e[17]-1.*e[7]*e[27]*e[9]-1.*e[7]*e[30]*e[12]+e[7]*e[28]*e[10]-1.*e[7]*e[32]*e[14]+e[10]*e[27]*e[6]+e[10]*e[0]*e[33]+e[10]*e[29]*e[8]+e[10]*e[2]*e[35]+e[28]*e[9]*e[6]+e[28]*e[0]*e[15]+e[28]*e[11]*e[8]+e[28]*e[2]*e[17]-1.*e[7]*e[29]*e[11];
+        A[56]=e[22]*e[18]*e[12]+e[22]*e[9]*e[21]+e[22]*e[20]*e[14]+e[22]*e[11]*e[23]+e[22]*e[19]*e[13]+e[25]*e[18]*e[15]+e[25]*e[9]*e[24]+e[25]*e[20]*e[17]+e[25]*e[11]*e[26]+e[25]*e[19]*e[16]+e[16]*e[18]*e[24]+e[16]*e[20]*e[26]+e[13]*e[18]*e[21]+e[13]*e[20]*e[23]+e[19]*e[18]*e[9]+e[19]*e[20]*e[11]-1.*e[19]*e[23]*e[14]-1.*e[19]*e[24]*e[15]-1.*e[19]*e[26]*e[17]-1.*e[19]*e[21]*e[12]+.5000000000*e[10]*ep2[22]+.5000000000*e[10]*ep2[25]+1.500000000*e[10]*ep2[19]+.5000000000*e[10]*ep2[18]+.5000000000*e[10]*ep2[20]-.5000000000*e[10]*ep2[26]-.5000000000*e[10]*ep2[23]-.5000000000*e[10]*ep2[24]-.5000000000*e[10]*ep2[21];
+        A[170]=e[19]*e[20]*e[26]-.5000000000*e[25]*ep2[20]+e[22]*e[21]*e[24]+e[19]*e[18]*e[24]+.5000000000*ep2[22]*e[25]-.5000000000*e[25]*ep2[21]-.5000000000*e[25]*ep2[23]+.5000000000*ep2[19]*e[25]-.5000000000*e[25]*ep2[18]+.5000000000*e[25]*ep2[24]+.5000000000*e[25]*ep2[26]+.5000000000*ep3[25]+e[22]*e[23]*e[26];
+        A[73]=-1.*e[20]*e[33]*e[6]-1.*e[20]*e[30]*e[3]-1.*e[20]*e[31]*e[4]-1.*e[29]*e[21]*e[3]-1.*e[29]*e[22]*e[4]-1.*e[29]*e[25]*e[7]-1.*e[29]*e[24]*e[6]+e[8]*e[27]*e[24]+e[8]*e[18]*e[33]+e[8]*e[28]*e[25]+e[8]*e[19]*e[34]+e[23]*e[27]*e[3]+e[23]*e[0]*e[30]+e[23]*e[28]*e[4]+e[23]*e[1]*e[31]+e[32]*e[18]*e[3]+e[32]*e[0]*e[21]+e[32]*e[19]*e[4]+e[32]*e[1]*e[22]+e[26]*e[27]*e[6]+e[26]*e[0]*e[33]+e[26]*e[28]*e[7]+e[26]*e[1]*e[34]+e[26]*e[29]*e[8]+e[26]*e[2]*e[35]+e[35]*e[18]*e[6]+e[35]*e[0]*e[24]+e[35]*e[19]*e[7]+e[35]*e[1]*e[25]+e[35]*e[20]*e[8]+e[2]*e[27]*e[18]+e[2]*e[28]*e[19]+3.*e[2]*e[29]*e[20]+e[20]*e[27]*e[0]+e[20]*e[28]*e[1]+e[29]*e[18]*e[0]+e[29]*e[19]*e[1]+e[5]*e[27]*e[21]+e[5]*e[18]*e[30]+e[5]*e[28]*e[22]+e[5]*e[19]*e[31]+e[5]*e[29]*e[23]+e[5]*e[20]*e[32]-1.*e[2]*e[33]*e[24]-1.*e[2]*e[30]*e[21]-1.*e[2]*e[31]*e[22]+e[2]*e[32]*e[23]-1.*e[2]*e[34]*e[25]-1.*e[20]*e[34]*e[7];
+        A[72]=e[5]*e[18]*e[3]+e[5]*e[0]*e[21]+e[5]*e[19]*e[4]+e[5]*e[1]*e[22]+e[5]*e[2]*e[23]+e[23]*e[1]*e[4]+e[23]*e[0]*e[3]+e[8]*e[18]*e[6]+e[8]*e[0]*e[24]+e[8]*e[19]*e[7]+e[8]*e[1]*e[25]+e[8]*e[2]*e[26]+e[26]*e[1]*e[7]+e[26]*e[0]*e[6]+e[2]*e[18]*e[0]+e[2]*e[19]*e[1]-1.*e[2]*e[21]*e[3]-1.*e[2]*e[22]*e[4]-1.*e[2]*e[25]*e[7]-1.*e[2]*e[24]*e[6]-.5000000000*e[20]*ep2[4]+.5000000000*e[20]*ep2[0]-.5000000000*e[20]*ep2[6]+.5000000000*e[20]*ep2[5]+.5000000000*e[20]*ep2[1]-.5000000000*e[20]*ep2[7]-.5000000000*e[20]*ep2[3]+1.500000000*e[20]*ep2[2]+.5000000000*e[20]*ep2[8];
+        A[75]=e[14]*e[9]*e[3]+e[14]*e[0]*e[12]+e[14]*e[10]*e[4]+e[14]*e[1]*e[13]+e[14]*e[11]*e[5]+e[17]*e[9]*e[6]+e[17]*e[0]*e[15]+e[17]*e[10]*e[7]+e[17]*e[1]*e[16]+e[17]*e[11]*e[8]+e[8]*e[9]*e[15]+e[8]*e[10]*e[16]+e[5]*e[9]*e[12]+e[5]*e[10]*e[13]+e[11]*e[9]*e[0]+e[11]*e[10]*e[1]-1.*e[11]*e[13]*e[4]-1.*e[11]*e[16]*e[7]-1.*e[11]*e[15]*e[6]-1.*e[11]*e[12]*e[3]+.5000000000*e[2]*ep2[14]+.5000000000*e[2]*ep2[17]+1.500000000*e[2]*ep2[11]+.5000000000*e[2]*ep2[9]+.5000000000*e[2]*ep2[10]-.5000000000*e[2]*ep2[16]-.5000000000*e[2]*ep2[12]-.5000000000*e[2]*ep2[15]-.5000000000*e[2]*ep2[13];
+        A[74]=e[14]*e[18]*e[12]+e[14]*e[9]*e[21]+e[14]*e[11]*e[23]+e[14]*e[19]*e[13]+e[14]*e[10]*e[22]+e[23]*e[9]*e[12]+e[23]*e[10]*e[13]+e[17]*e[18]*e[15]+e[17]*e[9]*e[24]+e[17]*e[11]*e[26]+e[17]*e[19]*e[16]+e[17]*e[10]*e[25]+e[26]*e[9]*e[15]+e[26]*e[10]*e[16]-1.*e[11]*e[24]*e[15]-1.*e[11]*e[25]*e[16]+e[11]*e[18]*e[9]-1.*e[11]*e[21]*e[12]+e[11]*e[19]*e[10]-1.*e[11]*e[22]*e[13]+1.500000000*e[20]*ep2[11]+.5000000000*e[20]*ep2[9]+.5000000000*e[20]*ep2[10]+.5000000000*e[20]*ep2[14]+.5000000000*e[20]*ep2[17]-.5000000000*e[20]*ep2[16]-.5000000000*e[20]*ep2[12]-.5000000000*e[20]*ep2[15]-.5000000000*e[20]*ep2[13];
+        A[77]=e[23]*e[10]*e[31]+e[32]*e[18]*e[12]+e[32]*e[9]*e[21]+e[32]*e[19]*e[13]+e[32]*e[10]*e[22]-1.*e[11]*e[33]*e[24]-1.*e[11]*e[30]*e[21]+e[11]*e[35]*e[26]-1.*e[11]*e[31]*e[22]-1.*e[11]*e[34]*e[25]+e[20]*e[27]*e[9]+e[20]*e[28]*e[10]+e[29]*e[18]*e[9]+e[29]*e[19]*e[10]+e[17]*e[27]*e[24]+e[17]*e[18]*e[33]+e[17]*e[28]*e[25]+e[17]*e[19]*e[34]+e[17]*e[29]*e[26]+e[17]*e[20]*e[35]-1.*e[20]*e[30]*e[12]-1.*e[20]*e[31]*e[13]-1.*e[20]*e[33]*e[15]-1.*e[20]*e[34]*e[16]-1.*e[29]*e[24]*e[15]-1.*e[29]*e[25]*e[16]-1.*e[29]*e[21]*e[12]-1.*e[29]*e[22]*e[13]+e[26]*e[27]*e[15]+e[26]*e[9]*e[33]+e[26]*e[28]*e[16]+e[26]*e[10]*e[34]+e[35]*e[18]*e[15]+e[35]*e[9]*e[24]+e[35]*e[19]*e[16]+e[35]*e[10]*e[25]+e[14]*e[27]*e[21]+e[14]*e[18]*e[30]+e[14]*e[28]*e[22]+e[14]*e[19]*e[31]+e[14]*e[29]*e[23]+e[14]*e[20]*e[32]+e[11]*e[27]*e[18]+e[11]*e[28]*e[19]+3.*e[11]*e[29]*e[20]+e[23]*e[27]*e[12]+e[23]*e[9]*e[30]+e[23]*e[11]*e[32]+e[23]*e[28]*e[13];
+        A[76]=e[23]*e[18]*e[12]+e[23]*e[9]*e[21]+e[23]*e[20]*e[14]+e[23]*e[19]*e[13]+e[23]*e[10]*e[22]+e[26]*e[18]*e[15]+e[26]*e[9]*e[24]+e[26]*e[20]*e[17]+e[26]*e[19]*e[16]+e[26]*e[10]*e[25]+e[17]*e[19]*e[25]+e[17]*e[18]*e[24]+e[14]*e[19]*e[22]+e[14]*e[18]*e[21]+e[20]*e[18]*e[9]+e[20]*e[19]*e[10]-1.*e[20]*e[24]*e[15]-1.*e[20]*e[25]*e[16]-1.*e[20]*e[21]*e[12]-1.*e[20]*e[22]*e[13]+.5000000000*e[11]*ep2[23]+.5000000000*e[11]*ep2[26]+.5000000000*e[11]*ep2[19]+.5000000000*e[11]*ep2[18]+1.500000000*e[11]*ep2[20]-.5000000000*e[11]*ep2[22]-.5000000000*e[11]*ep2[24]-.5000000000*e[11]*ep2[21]-.5000000000*e[11]*ep2[25];
+        A[79]=-1.*e[20]*e[21]*e[3]+e[20]*e[26]*e[8]-1.*e[20]*e[22]*e[4]-1.*e[20]*e[25]*e[7]-1.*e[20]*e[24]*e[6]+e[5]*e[19]*e[22]+e[5]*e[18]*e[21]+e[26]*e[18]*e[6]+e[26]*e[0]*e[24]+e[26]*e[19]*e[7]+e[26]*e[1]*e[25]+e[8]*e[19]*e[25]+e[8]*e[18]*e[24]+e[20]*e[18]*e[0]+e[20]*e[19]*e[1]+e[23]*e[18]*e[3]+e[23]*e[0]*e[21]+e[23]*e[19]*e[4]+e[23]*e[1]*e[22]+e[23]*e[20]*e[5]+1.500000000*ep2[20]*e[2]+.5000000000*e[2]*ep2[23]+.5000000000*e[2]*ep2[19]+.5000000000*e[2]*ep2[18]+.5000000000*e[2]*ep2[26]-.5000000000*e[2]*ep2[22]-.5000000000*e[2]*ep2[24]-.5000000000*e[2]*ep2[21]-.5000000000*e[2]*ep2[25];
+        A[78]=-1.*e[2]*e[15]*e[6]+e[2]*e[9]*e[0]-1.*e[2]*e[12]*e[3]+e[5]*e[9]*e[3]+e[5]*e[0]*e[12]+e[5]*e[10]*e[4]+e[5]*e[1]*e[13]+e[5]*e[2]*e[14]+e[14]*e[1]*e[4]+e[14]*e[0]*e[3]+e[8]*e[9]*e[6]+e[8]*e[0]*e[15]+e[8]*e[10]*e[7]+e[8]*e[1]*e[16]+e[8]*e[2]*e[17]+e[17]*e[1]*e[7]+e[17]*e[0]*e[6]+e[2]*e[10]*e[1]-1.*e[2]*e[13]*e[4]-1.*e[2]*e[16]*e[7]+.5000000000*e[11]*ep2[1]+.5000000000*e[11]*ep2[0]+1.500000000*e[11]*ep2[2]+.5000000000*e[11]*ep2[5]+.5000000000*e[11]*ep2[8]-.5000000000*e[11]*ep2[4]-.5000000000*e[11]*ep2[6]-.5000000000*e[11]*ep2[7]-.5000000000*e[11]*ep2[3];
+        A[64]=e[5]*e[19]*e[13]+e[5]*e[10]*e[22]+e[8]*e[18]*e[15]+e[8]*e[9]*e[24]+e[8]*e[20]*e[17]+e[8]*e[11]*e[26]+e[8]*e[19]*e[16]+e[8]*e[10]*e[25]+e[2]*e[18]*e[9]+e[2]*e[19]*e[10]-1.*e[11]*e[21]*e[3]-1.*e[11]*e[22]*e[4]-1.*e[11]*e[25]*e[7]-1.*e[11]*e[24]*e[6]+e[14]*e[18]*e[3]+e[14]*e[0]*e[21]+e[14]*e[19]*e[4]+e[14]*e[1]*e[22]+e[14]*e[2]*e[23]-1.*e[20]*e[13]*e[4]-1.*e[20]*e[16]*e[7]-1.*e[20]*e[15]*e[6]-1.*e[20]*e[12]*e[3]+e[23]*e[9]*e[3]+e[23]*e[0]*e[12]+e[23]*e[10]*e[4]+e[23]*e[1]*e[13]+e[17]*e[18]*e[6]+e[17]*e[0]*e[24]+e[17]*e[19]*e[7]+e[17]*e[1]*e[25]+e[17]*e[2]*e[26]-1.*e[2]*e[24]*e[15]-1.*e[2]*e[25]*e[16]-1.*e[2]*e[21]*e[12]-1.*e[2]*e[22]*e[13]+e[26]*e[9]*e[6]+e[26]*e[0]*e[15]+e[26]*e[10]*e[7]+e[26]*e[1]*e[16]+e[11]*e[18]*e[0]+e[11]*e[19]*e[1]+3.*e[11]*e[20]*e[2]+e[20]*e[9]*e[0]+e[20]*e[10]*e[1]+e[5]*e[18]*e[12]+e[5]*e[9]*e[21]+e[5]*e[20]*e[14]+e[5]*e[11]*e[23];
+        A[65]=e[32]*e[1]*e[4]+e[32]*e[0]*e[3]+e[8]*e[27]*e[6]+e[8]*e[0]*e[33]+e[8]*e[28]*e[7]+e[8]*e[1]*e[34]+e[35]*e[1]*e[7]+e[35]*e[0]*e[6]+e[2]*e[27]*e[0]+e[2]*e[28]*e[1]-1.*e[2]*e[34]*e[7]+e[2]*e[32]*e[5]-1.*e[2]*e[33]*e[6]-1.*e[2]*e[30]*e[3]+e[2]*e[35]*e[8]-1.*e[2]*e[31]*e[4]+e[5]*e[27]*e[3]+e[5]*e[0]*e[30]+e[5]*e[28]*e[4]+e[5]*e[1]*e[31]+1.500000000*e[29]*ep2[2]-.5000000000*e[29]*ep2[4]+.5000000000*e[29]*ep2[0]-.5000000000*e[29]*ep2[6]+.5000000000*e[29]*ep2[5]+.5000000000*e[29]*ep2[1]-.5000000000*e[29]*ep2[7]-.5000000000*e[29]*ep2[3]+.5000000000*e[29]*ep2[8];
+        A[66]=e[5]*e[0]*e[3]+e[8]*e[1]*e[7]+e[8]*e[0]*e[6]+e[5]*e[1]*e[4]-.5000000000*e[2]*ep2[4]+.5000000000*ep3[2]+.5000000000*e[2]*ep2[1]-.5000000000*e[2]*ep2[3]+.5000000000*e[2]*ep2[0]+.5000000000*e[2]*ep2[8]+.5000000000*e[2]*ep2[5]-.5000000000*e[2]*ep2[6]-.5000000000*e[2]*ep2[7];
+        A[67]=e[35]*e[9]*e[15]+e[35]*e[10]*e[16]-1.*e[11]*e[30]*e[12]-1.*e[11]*e[31]*e[13]-1.*e[11]*e[33]*e[15]-1.*e[11]*e[34]*e[16]+e[11]*e[27]*e[9]+e[11]*e[28]*e[10]+e[14]*e[27]*e[12]+e[14]*e[9]*e[30]+e[14]*e[11]*e[32]+e[14]*e[28]*e[13]+e[14]*e[10]*e[31]+e[32]*e[9]*e[12]+e[32]*e[10]*e[13]+e[17]*e[27]*e[15]+e[17]*e[9]*e[33]+e[17]*e[11]*e[35]+e[17]*e[28]*e[16]+e[17]*e[10]*e[34]+1.500000000*e[29]*ep2[11]-.5000000000*e[29]*ep2[16]+.5000000000*e[29]*ep2[9]-.5000000000*e[29]*ep2[12]-.5000000000*e[29]*ep2[15]+.5000000000*e[29]*ep2[17]+.5000000000*e[29]*ep2[10]+.5000000000*e[29]*ep2[14]-.5000000000*e[29]*ep2[13];
+        A[68]=e[14]*e[9]*e[12]+e[17]*e[10]*e[16]+e[17]*e[9]*e[15]+.5000000000*ep3[11]+e[14]*e[10]*e[13]+.5000000000*e[11]*ep2[10]-.5000000000*e[11]*ep2[15]+.5000000000*e[11]*ep2[14]-.5000000000*e[11]*ep2[13]-.5000000000*e[11]*ep2[12]+.5000000000*e[11]*ep2[9]-.5000000000*e[11]*ep2[16]+.5000000000*e[11]*ep2[17];
+        A[69]=e[20]*e[27]*e[18]+e[20]*e[28]*e[19]+e[23]*e[27]*e[21]+e[23]*e[18]*e[30]+e[23]*e[28]*e[22]+e[23]*e[19]*e[31]+e[23]*e[20]*e[32]+e[32]*e[19]*e[22]+e[32]*e[18]*e[21]+e[26]*e[27]*e[24]+e[26]*e[18]*e[33]+e[26]*e[28]*e[25]+e[26]*e[19]*e[34]+e[26]*e[20]*e[35]+e[35]*e[19]*e[25]+e[35]*e[18]*e[24]-1.*e[20]*e[33]*e[24]-1.*e[20]*e[30]*e[21]-1.*e[20]*e[31]*e[22]-1.*e[20]*e[34]*e[25]+.5000000000*e[29]*ep2[23]+.5000000000*e[29]*ep2[26]-.5000000000*e[29]*ep2[22]-.5000000000*e[29]*ep2[24]-.5000000000*e[29]*ep2[21]-.5000000000*e[29]*ep2[25]+1.500000000*e[29]*ep2[20]+.5000000000*e[29]*ep2[19]+.5000000000*e[29]*ep2[18];
+        A[70]=.5000000000*e[20]*ep2[26]+.5000000000*e[20]*ep2[18]+.5000000000*ep3[20]+.5000000000*e[20]*ep2[19]+e[26]*e[18]*e[24]+.5000000000*e[20]*ep2[23]-.5000000000*e[20]*ep2[25]+e[23]*e[19]*e[22]-.5000000000*e[20]*ep2[24]-.5000000000*e[20]*ep2[21]-.5000000000*e[20]*ep2[22]+e[23]*e[18]*e[21]+e[26]*e[19]*e[25];
+        A[71]=e[8]*e[28]*e[16]+e[8]*e[10]*e[34]+e[2]*e[27]*e[9]+3.*e[2]*e[29]*e[11]+e[2]*e[28]*e[10]+e[11]*e[27]*e[0]-1.*e[11]*e[34]*e[7]-1.*e[11]*e[33]*e[6]-1.*e[11]*e[30]*e[3]+e[11]*e[28]*e[1]-1.*e[11]*e[31]*e[4]+e[14]*e[27]*e[3]+e[14]*e[0]*e[30]+e[14]*e[28]*e[4]+e[14]*e[1]*e[31]+e[14]*e[2]*e[32]+e[29]*e[10]*e[1]-1.*e[29]*e[13]*e[4]-1.*e[29]*e[16]*e[7]-1.*e[29]*e[15]*e[6]+e[29]*e[9]*e[0]-1.*e[29]*e[12]*e[3]+e[32]*e[9]*e[3]+e[32]*e[0]*e[12]+e[32]*e[10]*e[4]+e[32]*e[1]*e[13]+e[17]*e[27]*e[6]+e[17]*e[0]*e[33]+e[17]*e[28]*e[7]+e[17]*e[1]*e[34]+e[17]*e[2]*e[35]-1.*e[2]*e[30]*e[12]-1.*e[2]*e[31]*e[13]-1.*e[2]*e[33]*e[15]-1.*e[2]*e[34]*e[16]+e[35]*e[9]*e[6]+e[35]*e[0]*e[15]+e[35]*e[10]*e[7]+e[35]*e[1]*e[16]+e[5]*e[27]*e[12]+e[5]*e[9]*e[30]+e[5]*e[29]*e[14]+e[5]*e[11]*e[32]+e[5]*e[28]*e[13]+e[5]*e[10]*e[31]+e[8]*e[27]*e[15]+e[8]*e[9]*e[33]+e[8]*e[29]*e[17]+e[8]*e[11]*e[35];
+        A[91]=-1.*e[12]*e[34]*e[7]+e[12]*e[32]*e[5]-1.*e[12]*e[35]*e[8]-1.*e[12]*e[29]*e[2]-1.*e[12]*e[28]*e[1]+e[12]*e[31]*e[4]-1.*e[30]*e[11]*e[2]-1.*e[30]*e[10]*e[1]+e[30]*e[13]*e[4]-1.*e[30]*e[16]*e[7]+e[30]*e[14]*e[5]-1.*e[30]*e[17]*e[8]+e[15]*e[3]*e[33]+e[15]*e[31]*e[7]+e[15]*e[4]*e[34]+e[15]*e[32]*e[8]+e[15]*e[5]*e[35]+e[3]*e[27]*e[9]-1.*e[3]*e[28]*e[10]-1.*e[3]*e[34]*e[16]-1.*e[3]*e[35]*e[17]-1.*e[3]*e[29]*e[11]+e[33]*e[13]*e[7]+e[33]*e[4]*e[16]+e[33]*e[14]*e[8]+e[33]*e[5]*e[17]+e[9]*e[28]*e[4]+e[9]*e[1]*e[31]+e[9]*e[29]*e[5]+e[9]*e[2]*e[32]+e[27]*e[10]*e[4]+e[27]*e[1]*e[13]+e[27]*e[11]*e[5]+e[27]*e[2]*e[14]+3.*e[3]*e[30]*e[12]+e[3]*e[32]*e[14]+e[3]*e[31]*e[13]+e[6]*e[30]*e[15]+e[6]*e[12]*e[33]+e[6]*e[32]*e[17]+e[6]*e[14]*e[35]+e[6]*e[31]*e[16]+e[6]*e[13]*e[34]+e[0]*e[27]*e[12]+e[0]*e[9]*e[30]+e[0]*e[29]*e[14]+e[0]*e[11]*e[32]+e[0]*e[28]*e[13]+e[0]*e[10]*e[31];
+        A[90]=.5000000000*e[21]*ep2[24]-.5000000000*e[21]*ep2[25]+.5000000000*e[21]*ep2[23]-.5000000000*e[21]*ep2[26]+.5000000000*ep2[18]*e[21]+.5000000000*e[21]*ep2[22]-.5000000000*e[21]*ep2[20]+e[24]*e[22]*e[25]+e[24]*e[23]*e[26]-.5000000000*e[21]*ep2[19]+e[18]*e[19]*e[22]+e[18]*e[20]*e[23]+.5000000000*ep3[21];
+        A[89]=-.5000000000*e[30]*ep2[26]-.5000000000*e[30]*ep2[19]-.5000000000*e[30]*ep2[20]-.5000000000*e[30]*ep2[25]+.5000000000*ep2[18]*e[30]+1.500000000*e[30]*ep2[21]+.5000000000*e[30]*ep2[22]+.5000000000*e[30]*ep2[23]+.5000000000*e[30]*ep2[24]+e[18]*e[27]*e[21]+e[18]*e[28]*e[22]+e[18]*e[19]*e[31]+e[18]*e[29]*e[23]+e[18]*e[20]*e[32]+e[27]*e[19]*e[22]+e[27]*e[20]*e[23]+e[21]*e[31]*e[22]+e[21]*e[32]*e[23]+e[24]*e[21]*e[33]+e[24]*e[31]*e[25]+e[24]*e[22]*e[34]+e[24]*e[32]*e[26]+e[24]*e[23]*e[35]+e[33]*e[22]*e[25]+e[33]*e[23]*e[26]-1.*e[21]*e[29]*e[20]-1.*e[21]*e[35]*e[26]-1.*e[21]*e[28]*e[19]-1.*e[21]*e[34]*e[25];
+        A[88]=.5000000000*e[12]*ep2[15]-.5000000000*e[12]*ep2[17]+e[15]*e[13]*e[16]-.5000000000*e[12]*ep2[10]+e[15]*e[14]*e[17]-.5000000000*e[12]*ep2[16]-.5000000000*e[12]*ep2[11]+e[9]*e[10]*e[13]+.5000000000*e[12]*ep2[13]+.5000000000*ep2[9]*e[12]+.5000000000*ep3[12]+e[9]*e[11]*e[14]+.5000000000*e[12]*ep2[14];
+        A[95]=e[12]*e[13]*e[4]+e[12]*e[14]*e[5]+e[15]*e[12]*e[6]+e[15]*e[13]*e[7]+e[15]*e[4]*e[16]+e[15]*e[14]*e[8]+e[15]*e[5]*e[17]+e[6]*e[14]*e[17]+e[6]*e[13]*e[16]+e[0]*e[11]*e[14]+e[0]*e[9]*e[12]+e[0]*e[10]*e[13]+e[9]*e[10]*e[4]+e[9]*e[1]*e[13]+e[9]*e[11]*e[5]+e[9]*e[2]*e[14]-1.*e[12]*e[11]*e[2]-1.*e[12]*e[10]*e[1]-1.*e[12]*e[16]*e[7]-1.*e[12]*e[17]*e[8]+1.500000000*ep2[12]*e[3]+.5000000000*e[3]*ep2[15]-.5000000000*e[3]*ep2[16]+.5000000000*e[3]*ep2[9]-.5000000000*e[3]*ep2[11]-.5000000000*e[3]*ep2[17]-.5000000000*e[3]*ep2[10]+.5000000000*e[3]*ep2[14]+.5000000000*e[3]*ep2[13];
+        A[94]=e[18]*e[11]*e[14]+e[18]*e[9]*e[12]+e[18]*e[10]*e[13]+e[12]*e[23]*e[14]+e[12]*e[22]*e[13]+e[15]*e[12]*e[24]+e[15]*e[23]*e[17]+e[15]*e[14]*e[26]+e[15]*e[22]*e[16]+e[15]*e[13]*e[25]+e[24]*e[14]*e[17]+e[24]*e[13]*e[16]-1.*e[12]*e[25]*e[16]-1.*e[12]*e[26]*e[17]-1.*e[12]*e[20]*e[11]-1.*e[12]*e[19]*e[10]+e[9]*e[20]*e[14]+e[9]*e[11]*e[23]+e[9]*e[19]*e[13]+e[9]*e[10]*e[22]+.5000000000*ep2[9]*e[21]-.5000000000*e[21]*ep2[16]-.5000000000*e[21]*ep2[11]-.5000000000*e[21]*ep2[17]-.5000000000*e[21]*ep2[10]+1.500000000*e[21]*ep2[12]+.5000000000*e[21]*ep2[14]+.5000000000*e[21]*ep2[13]+.5000000000*e[21]*ep2[15];
+        A[93]=-1.*e[21]*e[35]*e[8]-1.*e[21]*e[29]*e[2]-1.*e[21]*e[28]*e[1]+e[21]*e[31]*e[4]-1.*e[30]*e[26]*e[8]-1.*e[30]*e[20]*e[2]-1.*e[30]*e[19]*e[1]+e[30]*e[22]*e[4]-1.*e[30]*e[25]*e[7]+e[30]*e[23]*e[5]+e[6]*e[31]*e[25]+e[6]*e[22]*e[34]+e[6]*e[32]*e[26]+e[6]*e[23]*e[35]+e[24]*e[30]*e[6]+e[24]*e[3]*e[33]+e[24]*e[31]*e[7]+e[24]*e[4]*e[34]+e[24]*e[32]*e[8]+e[24]*e[5]*e[35]+e[33]*e[21]*e[6]+e[33]*e[22]*e[7]+e[33]*e[4]*e[25]+e[33]*e[23]*e[8]+e[33]*e[5]*e[26]+e[0]*e[27]*e[21]+e[0]*e[18]*e[30]+e[0]*e[28]*e[22]+e[0]*e[19]*e[31]+e[0]*e[29]*e[23]+e[0]*e[20]*e[32]+e[18]*e[27]*e[3]+e[18]*e[28]*e[4]+e[18]*e[1]*e[31]+e[18]*e[29]*e[5]+e[18]*e[2]*e[32]+e[27]*e[19]*e[4]+e[27]*e[1]*e[22]+e[27]*e[20]*e[5]+e[27]*e[2]*e[23]+3.*e[3]*e[30]*e[21]+e[3]*e[31]*e[22]+e[3]*e[32]*e[23]-1.*e[3]*e[29]*e[20]-1.*e[3]*e[35]*e[26]-1.*e[3]*e[28]*e[19]-1.*e[3]*e[34]*e[25]-1.*e[21]*e[34]*e[7]+e[21]*e[32]*e[5];
+        A[92]=e[18]*e[1]*e[4]+e[18]*e[0]*e[3]+e[18]*e[2]*e[5]+e[3]*e[22]*e[4]+e[3]*e[23]*e[5]+e[6]*e[3]*e[24]+e[6]*e[22]*e[7]+e[6]*e[4]*e[25]+e[6]*e[23]*e[8]+e[6]*e[5]*e[26]+e[24]*e[4]*e[7]+e[24]*e[5]*e[8]+e[0]*e[19]*e[4]+e[0]*e[1]*e[22]+e[0]*e[20]*e[5]+e[0]*e[2]*e[23]-1.*e[3]*e[26]*e[8]-1.*e[3]*e[20]*e[2]-1.*e[3]*e[19]*e[1]-1.*e[3]*e[25]*e[7]+.5000000000*e[21]*ep2[4]+.5000000000*e[21]*ep2[0]+.5000000000*e[21]*ep2[6]+.5000000000*e[21]*ep2[5]-.5000000000*e[21]*ep2[1]-.5000000000*e[21]*ep2[7]+1.500000000*e[21]*ep2[3]-.5000000000*e[21]*ep2[2]-.5000000000*e[21]*ep2[8];
+        A[82]=.5000000000*ep2[27]*e[21]+1.500000000*e[21]*ep2[30]+.5000000000*e[21]*ep2[32]+.5000000000*e[21]*ep2[31]+.5000000000*e[21]*ep2[33]-.5000000000*e[21]*ep2[28]-.5000000000*e[21]*ep2[29]-.5000000000*e[21]*ep2[34]-.5000000000*e[21]*ep2[35]+e[18]*e[27]*e[30]+e[18]*e[29]*e[32]+e[18]*e[28]*e[31]+e[27]*e[28]*e[22]+e[27]*e[19]*e[31]+e[27]*e[29]*e[23]+e[27]*e[20]*e[32]+e[30]*e[31]*e[22]+e[30]*e[32]*e[23]+e[24]*e[30]*e[33]+e[24]*e[32]*e[35]+e[24]*e[31]*e[34]+e[33]*e[31]*e[25]+e[33]*e[22]*e[34]+e[33]*e[32]*e[26]+e[33]*e[23]*e[35]-1.*e[30]*e[29]*e[20]-1.*e[30]*e[35]*e[26]-1.*e[30]*e[28]*e[19]-1.*e[30]*e[34]*e[25];
+        A[192]=-.5000000000*e[26]*ep2[4]-.5000000000*e[26]*ep2[0]+.5000000000*e[26]*ep2[6]+.5000000000*e[26]*ep2[5]-.5000000000*e[26]*ep2[1]+.5000000000*e[26]*ep2[7]-.5000000000*e[26]*ep2[3]+.5000000000*e[26]*ep2[2]+1.500000000*e[26]*ep2[8]+e[20]*e[0]*e[6]+e[20]*e[2]*e[8]+e[5]*e[21]*e[6]+e[5]*e[3]*e[24]+e[5]*e[22]*e[7]+e[5]*e[4]*e[25]+e[5]*e[23]*e[8]+e[23]*e[4]*e[7]+e[23]*e[3]*e[6]+e[8]*e[24]*e[6]+e[8]*e[25]*e[7]+e[2]*e[18]*e[6]+e[2]*e[0]*e[24]+e[2]*e[19]*e[7]+e[2]*e[1]*e[25]-1.*e[8]*e[21]*e[3]-1.*e[8]*e[19]*e[1]-1.*e[8]*e[22]*e[4]-1.*e[8]*e[18]*e[0]+e[20]*e[1]*e[7];
+        A[83]=e[9]*e[27]*e[30]+e[9]*e[29]*e[32]+e[9]*e[28]*e[31]+e[33]*e[30]*e[15]+e[33]*e[32]*e[17]+e[33]*e[14]*e[35]+e[33]*e[31]*e[16]+e[33]*e[13]*e[34]+e[27]*e[29]*e[14]+e[27]*e[11]*e[32]+e[27]*e[28]*e[13]+e[27]*e[10]*e[31]-1.*e[30]*e[28]*e[10]+e[30]*e[31]*e[13]+e[30]*e[32]*e[14]-1.*e[30]*e[34]*e[16]-1.*e[30]*e[35]*e[17]-1.*e[30]*e[29]*e[11]+e[15]*e[32]*e[35]+e[15]*e[31]*e[34]-.5000000000*e[12]*ep2[34]-.5000000000*e[12]*ep2[35]+.5000000000*e[12]*ep2[27]+.5000000000*e[12]*ep2[32]-.5000000000*e[12]*ep2[28]-.5000000000*e[12]*ep2[29]+.5000000000*e[12]*ep2[31]+.5000000000*e[12]*ep2[33]+1.500000000*e[12]*ep2[30];
+        A[193]=e[23]*e[30]*e[6]+e[23]*e[3]*e[33]+e[23]*e[31]*e[7]+e[23]*e[4]*e[34]+e[32]*e[21]*e[6]+e[32]*e[3]*e[24]+e[32]*e[22]*e[7]+e[32]*e[4]*e[25]+e[26]*e[33]*e[6]+e[26]*e[34]*e[7]+3.*e[26]*e[35]*e[8]+e[35]*e[24]*e[6]+e[35]*e[25]*e[7]+e[2]*e[27]*e[24]+e[2]*e[18]*e[33]+e[2]*e[28]*e[25]+e[2]*e[19]*e[34]+e[2]*e[29]*e[26]+e[2]*e[20]*e[35]+e[20]*e[27]*e[6]+e[20]*e[0]*e[33]+e[20]*e[28]*e[7]+e[20]*e[1]*e[34]+e[20]*e[29]*e[8]+e[29]*e[18]*e[6]+e[29]*e[0]*e[24]+e[29]*e[19]*e[7]+e[29]*e[1]*e[25]+e[5]*e[30]*e[24]+e[5]*e[21]*e[33]+e[5]*e[31]*e[25]+e[5]*e[22]*e[34]+e[5]*e[32]*e[26]+e[5]*e[23]*e[35]-1.*e[8]*e[27]*e[18]+e[8]*e[33]*e[24]-1.*e[8]*e[30]*e[21]-1.*e[8]*e[31]*e[22]+e[8]*e[32]*e[23]-1.*e[8]*e[28]*e[19]+e[8]*e[34]*e[25]-1.*e[26]*e[27]*e[0]-1.*e[26]*e[30]*e[3]-1.*e[26]*e[28]*e[1]-1.*e[26]*e[31]*e[4]-1.*e[35]*e[21]*e[3]-1.*e[35]*e[19]*e[1]-1.*e[35]*e[22]*e[4]-1.*e[35]*e[18]*e[0];
+        A[80]=e[27]*e[29]*e[32]+e[27]*e[28]*e[31]+e[33]*e[32]*e[35]+e[33]*e[31]*e[34]+.5000000000*ep3[30]-.5000000000*e[30]*ep2[28]-.5000000000*e[30]*ep2[29]-.5000000000*e[30]*ep2[34]+.5000000000*e[30]*ep2[33]+.5000000000*ep2[27]*e[30]+.5000000000*e[30]*ep2[32]+.5000000000*e[30]*ep2[31]-.5000000000*e[30]*ep2[35];
+        A[194]=.5000000000*ep2[14]*e[26]+1.500000000*e[26]*ep2[17]+.5000000000*e[26]*ep2[15]+.5000000000*e[26]*ep2[16]+.5000000000*ep2[11]*e[26]-.5000000000*e[26]*ep2[9]-.5000000000*e[26]*ep2[12]-.5000000000*e[26]*ep2[10]-.5000000000*e[26]*ep2[13]+e[20]*e[11]*e[17]+e[20]*e[9]*e[15]+e[20]*e[10]*e[16]+e[14]*e[21]*e[15]+e[14]*e[12]*e[24]+e[14]*e[23]*e[17]+e[14]*e[22]*e[16]+e[14]*e[13]*e[25]+e[23]*e[12]*e[15]+e[23]*e[13]*e[16]+e[17]*e[24]*e[15]+e[17]*e[25]*e[16]-1.*e[17]*e[18]*e[9]-1.*e[17]*e[21]*e[12]-1.*e[17]*e[19]*e[10]-1.*e[17]*e[22]*e[13]+e[11]*e[18]*e[15]+e[11]*e[9]*e[24]+e[11]*e[19]*e[16]+e[11]*e[10]*e[25];
+        A[81]=e[0]*e[27]*e[30]+e[0]*e[29]*e[32]+e[0]*e[28]*e[31]+e[30]*e[31]*e[4]+e[30]*e[32]*e[5]+e[6]*e[30]*e[33]+e[6]*e[32]*e[35]+e[6]*e[31]*e[34]+e[27]*e[28]*e[4]+e[27]*e[1]*e[31]+e[27]*e[29]*e[5]+e[27]*e[2]*e[32]+e[33]*e[31]*e[7]+e[33]*e[4]*e[34]+e[33]*e[32]*e[8]+e[33]*e[5]*e[35]-1.*e[30]*e[34]*e[7]-1.*e[30]*e[35]*e[8]-1.*e[30]*e[29]*e[2]-1.*e[30]*e[28]*e[1]+1.500000000*e[3]*ep2[30]+.5000000000*e[3]*ep2[32]+.5000000000*e[3]*ep2[31]+.5000000000*e[3]*ep2[27]-.5000000000*e[3]*ep2[28]-.5000000000*e[3]*ep2[29]+.5000000000*e[3]*ep2[33]-.5000000000*e[3]*ep2[34]-.5000000000*e[3]*ep2[35];
+        A[195]=.5000000000*ep2[14]*e[8]+1.500000000*ep2[17]*e[8]+.5000000000*e[8]*ep2[15]+.5000000000*e[8]*ep2[16]-.5000000000*e[8]*ep2[9]+.5000000000*e[8]*ep2[11]-.5000000000*e[8]*ep2[12]-.5000000000*e[8]*ep2[10]-.5000000000*e[8]*ep2[13]+e[14]*e[12]*e[6]+e[14]*e[3]*e[15]+e[14]*e[13]*e[7]+e[14]*e[4]*e[16]+e[14]*e[5]*e[17]+e[17]*e[15]*e[6]+e[17]*e[16]*e[7]+e[2]*e[11]*e[17]+e[2]*e[9]*e[15]+e[2]*e[10]*e[16]+e[5]*e[12]*e[15]+e[5]*e[13]*e[16]+e[11]*e[9]*e[6]+e[11]*e[0]*e[15]+e[11]*e[10]*e[7]+e[11]*e[1]*e[16]-1.*e[17]*e[10]*e[1]-1.*e[17]*e[13]*e[4]-1.*e[17]*e[9]*e[0]-1.*e[17]*e[12]*e[3];
+        A[86]=-.5000000000*e[3]*ep2[1]-.5000000000*e[3]*ep2[7]+.5000000000*ep3[3]-.5000000000*e[3]*ep2[8]+e[0]*e[2]*e[5]+.5000000000*e[3]*ep2[6]+.5000000000*e[3]*ep2[4]-.5000000000*e[3]*ep2[2]+e[0]*e[1]*e[4]+e[6]*e[4]*e[7]+.5000000000*ep2[0]*e[3]+.5000000000*e[3]*ep2[5]+e[6]*e[5]*e[8];
+        A[196]=.5000000000*ep2[23]*e[17]+1.500000000*ep2[26]*e[17]+.5000000000*e[17]*ep2[25]+.5000000000*e[17]*ep2[24]-.5000000000*e[17]*ep2[18]-.5000000000*e[17]*ep2[19]+.5000000000*e[17]*ep2[20]-.5000000000*e[17]*ep2[22]-.5000000000*e[17]*ep2[21]+e[23]*e[21]*e[15]+e[23]*e[12]*e[24]+e[23]*e[14]*e[26]+e[23]*e[22]*e[16]+e[23]*e[13]*e[25]+e[26]*e[24]*e[15]+e[26]*e[25]*e[16]+e[11]*e[19]*e[25]+e[11]*e[18]*e[24]+e[11]*e[20]*e[26]+e[14]*e[22]*e[25]+e[14]*e[21]*e[24]+e[20]*e[18]*e[15]+e[20]*e[9]*e[24]+e[20]*e[19]*e[16]+e[20]*e[10]*e[25]-1.*e[26]*e[18]*e[9]-1.*e[26]*e[21]*e[12]-1.*e[26]*e[19]*e[10]-1.*e[26]*e[22]*e[13];
+        A[87]=-1.*e[12]*e[34]*e[16]-1.*e[12]*e[35]*e[17]-1.*e[12]*e[29]*e[11]+e[9]*e[27]*e[12]+e[9]*e[29]*e[14]+e[9]*e[11]*e[32]+e[9]*e[28]*e[13]+e[9]*e[10]*e[31]+e[27]*e[11]*e[14]+e[27]*e[10]*e[13]+e[12]*e[32]*e[14]+e[12]*e[31]*e[13]+e[15]*e[12]*e[33]+e[15]*e[32]*e[17]+e[15]*e[14]*e[35]+e[15]*e[31]*e[16]+e[15]*e[13]*e[34]+e[33]*e[14]*e[17]+e[33]*e[13]*e[16]-1.*e[12]*e[28]*e[10]+.5000000000*ep2[9]*e[30]-.5000000000*e[30]*ep2[16]-.5000000000*e[30]*ep2[11]+1.500000000*e[30]*ep2[12]+.5000000000*e[30]*ep2[15]-.5000000000*e[30]*ep2[17]-.5000000000*e[30]*ep2[10]+.5000000000*e[30]*ep2[14]+.5000000000*e[30]*ep2[13];
+        A[197]=e[32]*e[22]*e[16]+e[32]*e[13]*e[25]-1.*e[17]*e[27]*e[18]+e[17]*e[33]*e[24]-1.*e[17]*e[30]*e[21]+e[17]*e[29]*e[20]+3.*e[17]*e[35]*e[26]-1.*e[17]*e[31]*e[22]-1.*e[17]*e[28]*e[19]+e[17]*e[34]*e[25]+e[20]*e[27]*e[15]+e[20]*e[9]*e[33]+e[20]*e[28]*e[16]+e[20]*e[10]*e[34]+e[29]*e[18]*e[15]+e[29]*e[9]*e[24]+e[29]*e[19]*e[16]+e[29]*e[10]*e[25]-1.*e[26]*e[27]*e[9]-1.*e[26]*e[30]*e[12]-1.*e[26]*e[28]*e[10]-1.*e[26]*e[31]*e[13]+e[26]*e[33]*e[15]+e[26]*e[34]*e[16]+e[35]*e[24]*e[15]+e[35]*e[25]*e[16]-1.*e[35]*e[18]*e[9]-1.*e[35]*e[21]*e[12]-1.*e[35]*e[19]*e[10]-1.*e[35]*e[22]*e[13]+e[14]*e[30]*e[24]+e[14]*e[21]*e[33]+e[14]*e[31]*e[25]+e[14]*e[22]*e[34]+e[14]*e[32]*e[26]+e[14]*e[23]*e[35]+e[11]*e[27]*e[24]+e[11]*e[18]*e[33]+e[11]*e[28]*e[25]+e[11]*e[19]*e[34]+e[11]*e[29]*e[26]+e[11]*e[20]*e[35]+e[23]*e[30]*e[15]+e[23]*e[12]*e[33]+e[23]*e[32]*e[17]+e[23]*e[31]*e[16]+e[23]*e[13]*e[34]+e[32]*e[21]*e[15]+e[32]*e[12]*e[24];
+        A[84]=e[6]*e[23]*e[17]+e[6]*e[14]*e[26]+e[6]*e[22]*e[16]+e[6]*e[13]*e[25]+e[0]*e[20]*e[14]+e[0]*e[11]*e[23]+e[0]*e[19]*e[13]+e[0]*e[10]*e[22]-1.*e[12]*e[26]*e[8]-1.*e[12]*e[20]*e[2]-1.*e[12]*e[19]*e[1]+e[12]*e[22]*e[4]-1.*e[12]*e[25]*e[7]+e[12]*e[23]*e[5]-1.*e[21]*e[11]*e[2]-1.*e[21]*e[10]*e[1]+e[21]*e[13]*e[4]-1.*e[21]*e[16]*e[7]+e[21]*e[14]*e[5]-1.*e[21]*e[17]*e[8]+e[15]*e[3]*e[24]+e[15]*e[22]*e[7]+e[15]*e[4]*e[25]+e[15]*e[23]*e[8]+e[15]*e[5]*e[26]-1.*e[3]*e[25]*e[16]-1.*e[3]*e[26]*e[17]-1.*e[3]*e[20]*e[11]-1.*e[3]*e[19]*e[10]+e[24]*e[13]*e[7]+e[24]*e[4]*e[16]+e[24]*e[14]*e[8]+e[24]*e[5]*e[17]+e[9]*e[18]*e[3]+e[9]*e[0]*e[21]+e[9]*e[19]*e[4]+e[9]*e[1]*e[22]+e[9]*e[20]*e[5]+e[9]*e[2]*e[23]+e[18]*e[0]*e[12]+e[18]*e[10]*e[4]+e[18]*e[1]*e[13]+e[18]*e[11]*e[5]+e[18]*e[2]*e[14]+3.*e[3]*e[21]*e[12]+e[3]*e[23]*e[14]+e[3]*e[22]*e[13]+e[6]*e[21]*e[15]+e[6]*e[12]*e[24];
+        A[198]=.5000000000*ep2[5]*e[17]+1.500000000*e[17]*ep2[8]+.5000000000*e[17]*ep2[7]+.5000000000*e[17]*ep2[6]+.5000000000*ep2[2]*e[17]-.5000000000*e[17]*ep2[4]-.5000000000*e[17]*ep2[0]-.5000000000*e[17]*ep2[1]-.5000000000*e[17]*ep2[3]+e[11]*e[1]*e[7]+e[11]*e[0]*e[6]+e[11]*e[2]*e[8]+e[5]*e[12]*e[6]+e[5]*e[3]*e[15]+e[5]*e[13]*e[7]+e[5]*e[4]*e[16]+e[5]*e[14]*e[8]+e[14]*e[4]*e[7]+e[14]*e[3]*e[6]+e[8]*e[15]*e[6]+e[8]*e[16]*e[7]-1.*e[8]*e[10]*e[1]-1.*e[8]*e[13]*e[4]-1.*e[8]*e[9]*e[0]-1.*e[8]*e[12]*e[3]+e[2]*e[9]*e[6]+e[2]*e[0]*e[15]+e[2]*e[10]*e[7]+e[2]*e[1]*e[16];
+        A[85]=e[6]*e[4]*e[34]+e[6]*e[32]*e[8]+e[6]*e[5]*e[35]+e[33]*e[4]*e[7]+e[33]*e[5]*e[8]+e[0]*e[27]*e[3]+e[0]*e[28]*e[4]+e[0]*e[1]*e[31]+e[0]*e[29]*e[5]+e[0]*e[2]*e[32]-1.*e[3]*e[34]*e[7]+e[3]*e[32]*e[5]+e[3]*e[33]*e[6]-1.*e[3]*e[35]*e[8]-1.*e[3]*e[29]*e[2]-1.*e[3]*e[28]*e[1]+e[3]*e[31]*e[4]+e[27]*e[1]*e[4]+e[27]*e[2]*e[5]+e[6]*e[31]*e[7]+.5000000000*e[30]*ep2[4]+.5000000000*e[30]*ep2[6]+.5000000000*e[30]*ep2[5]-.5000000000*e[30]*ep2[1]-.5000000000*e[30]*ep2[7]-.5000000000*e[30]*ep2[2]-.5000000000*e[30]*ep2[8]+.5000000000*ep2[0]*e[30]+1.500000000*e[30]*ep2[3];
+        A[199]=.5000000000*ep2[23]*e[8]+1.500000000*ep2[26]*e[8]-.5000000000*e[8]*ep2[18]-.5000000000*e[8]*ep2[19]-.5000000000*e[8]*ep2[22]+.5000000000*e[8]*ep2[24]-.5000000000*e[8]*ep2[21]+.5000000000*e[8]*ep2[25]+.5000000000*ep2[20]*e[8]+e[20]*e[18]*e[6]+e[20]*e[0]*e[24]+e[20]*e[19]*e[7]+e[20]*e[1]*e[25]+e[20]*e[2]*e[26]+e[23]*e[21]*e[6]+e[23]*e[3]*e[24]+e[23]*e[22]*e[7]+e[23]*e[4]*e[25]+e[23]*e[5]*e[26]-1.*e[26]*e[21]*e[3]-1.*e[26]*e[19]*e[1]-1.*e[26]*e[22]*e[4]-1.*e[26]*e[18]*e[0]+e[26]*e[25]*e[7]+e[26]*e[24]*e[6]+e[2]*e[19]*e[25]+e[2]*e[18]*e[24]+e[5]*e[22]*e[25]+e[5]*e[21]*e[24];
+        A[109]=e[19]*e[27]*e[21]+e[19]*e[18]*e[30]+e[19]*e[28]*e[22]+e[19]*e[29]*e[23]+e[19]*e[20]*e[32]+e[28]*e[18]*e[21]+e[28]*e[20]*e[23]+e[22]*e[30]*e[21]+e[22]*e[32]*e[23]+e[25]*e[30]*e[24]+e[25]*e[21]*e[33]+e[25]*e[22]*e[34]+e[25]*e[32]*e[26]+e[25]*e[23]*e[35]+e[34]*e[21]*e[24]+e[34]*e[23]*e[26]-1.*e[22]*e[27]*e[18]-1.*e[22]*e[33]*e[24]-1.*e[22]*e[29]*e[20]-1.*e[22]*e[35]*e[26]+.5000000000*ep2[19]*e[31]+1.500000000*e[31]*ep2[22]+.5000000000*e[31]*ep2[21]+.5000000000*e[31]*ep2[23]+.5000000000*e[31]*ep2[25]-.5000000000*e[31]*ep2[26]-.5000000000*e[31]*ep2[18]-.5000000000*e[31]*ep2[20]-.5000000000*e[31]*ep2[24];
+        A[108]=-.5000000000*e[13]*ep2[15]+.5000000000*e[13]*ep2[16]+.5000000000*e[13]*ep2[12]+e[16]*e[12]*e[15]+.5000000000*ep3[13]+e[10]*e[11]*e[14]+.5000000000*e[13]*ep2[14]-.5000000000*e[13]*ep2[17]-.5000000000*e[13]*ep2[11]-.5000000000*e[13]*ep2[9]+.5000000000*ep2[10]*e[13]+e[10]*e[9]*e[12]+e[16]*e[14]*e[17];
+        A[111]=-1.*e[13]*e[29]*e[2]-1.*e[31]*e[11]*e[2]-1.*e[31]*e[15]*e[6]-1.*e[31]*e[9]*e[0]+e[31]*e[14]*e[5]+e[31]*e[12]*e[3]-1.*e[31]*e[17]*e[8]+e[16]*e[30]*e[6]+e[16]*e[3]*e[33]+e[16]*e[4]*e[34]+e[16]*e[32]*e[8]+e[16]*e[5]*e[35]-1.*e[4]*e[27]*e[9]+e[4]*e[28]*e[10]-1.*e[4]*e[33]*e[15]-1.*e[4]*e[35]*e[17]-1.*e[4]*e[29]*e[11]+e[34]*e[12]*e[6]+e[34]*e[3]*e[15]+e[34]*e[14]*e[8]+e[34]*e[5]*e[17]+e[10]*e[27]*e[3]+e[10]*e[0]*e[30]+e[10]*e[29]*e[5]+e[10]*e[2]*e[32]+e[28]*e[9]*e[3]+e[28]*e[0]*e[12]+e[28]*e[11]*e[5]+e[28]*e[2]*e[14]+e[4]*e[30]*e[12]+e[4]*e[32]*e[14]+3.*e[4]*e[31]*e[13]+e[7]*e[30]*e[15]+e[7]*e[12]*e[33]+e[7]*e[32]*e[17]+e[7]*e[14]*e[35]+e[7]*e[31]*e[16]+e[7]*e[13]*e[34]+e[1]*e[27]*e[12]+e[1]*e[9]*e[30]+e[1]*e[29]*e[14]+e[1]*e[11]*e[32]+e[1]*e[28]*e[13]+e[1]*e[10]*e[31]-1.*e[13]*e[27]*e[0]+e[13]*e[32]*e[5]-1.*e[13]*e[33]*e[6]+e[13]*e[30]*e[3]-1.*e[13]*e[35]*e[8];
+        A[110]=e[25]*e[23]*e[26]+e[19]*e[20]*e[23]+e[19]*e[18]*e[21]+e[25]*e[21]*e[24]+.5000000000*ep3[22]+.5000000000*e[22]*ep2[23]+.5000000000*ep2[19]*e[22]-.5000000000*e[22]*ep2[18]-.5000000000*e[22]*ep2[24]+.5000000000*e[22]*ep2[21]+.5000000000*e[22]*ep2[25]-.5000000000*e[22]*ep2[20]-.5000000000*e[22]*ep2[26];
+        A[105]=e[34]*e[5]*e[8]+e[1]*e[27]*e[3]+e[1]*e[0]*e[30]+e[1]*e[28]*e[4]+e[1]*e[29]*e[5]+e[1]*e[2]*e[32]-1.*e[4]*e[27]*e[0]+e[4]*e[34]*e[7]+e[4]*e[32]*e[5]-1.*e[4]*e[33]*e[6]+e[4]*e[30]*e[3]-1.*e[4]*e[35]*e[8]-1.*e[4]*e[29]*e[2]+e[28]*e[0]*e[3]+e[28]*e[2]*e[5]+e[7]*e[30]*e[6]+e[7]*e[3]*e[33]+e[7]*e[32]*e[8]+e[7]*e[5]*e[35]+e[34]*e[3]*e[6]+.5000000000*ep2[1]*e[31]+1.500000000*e[31]*ep2[4]-.5000000000*e[31]*ep2[0]-.5000000000*e[31]*ep2[6]+.5000000000*e[31]*ep2[5]+.5000000000*e[31]*ep2[7]+.5000000000*e[31]*ep2[3]-.5000000000*e[31]*ep2[2]-.5000000000*e[31]*ep2[8];
+        A[104]=e[1]*e[20]*e[14]+e[1]*e[11]*e[23]+e[13]*e[21]*e[3]-1.*e[13]*e[26]*e[8]-1.*e[13]*e[20]*e[2]-1.*e[13]*e[18]*e[0]+e[13]*e[23]*e[5]-1.*e[13]*e[24]*e[6]-1.*e[22]*e[11]*e[2]-1.*e[22]*e[15]*e[6]-1.*e[22]*e[9]*e[0]+e[22]*e[14]*e[5]+e[22]*e[12]*e[3]-1.*e[22]*e[17]*e[8]+e[16]*e[21]*e[6]+e[16]*e[3]*e[24]+e[16]*e[4]*e[25]+e[16]*e[23]*e[8]+e[16]*e[5]*e[26]-1.*e[4]*e[24]*e[15]-1.*e[4]*e[26]*e[17]-1.*e[4]*e[20]*e[11]-1.*e[4]*e[18]*e[9]+e[25]*e[12]*e[6]+e[25]*e[3]*e[15]+e[25]*e[14]*e[8]+e[25]*e[5]*e[17]+e[10]*e[18]*e[3]+e[10]*e[0]*e[21]+e[10]*e[19]*e[4]+e[10]*e[1]*e[22]+e[10]*e[20]*e[5]+e[10]*e[2]*e[23]+e[19]*e[9]*e[3]+e[19]*e[0]*e[12]+e[19]*e[1]*e[13]+e[19]*e[11]*e[5]+e[19]*e[2]*e[14]+e[4]*e[21]*e[12]+e[4]*e[23]*e[14]+3.*e[4]*e[22]*e[13]+e[7]*e[21]*e[15]+e[7]*e[12]*e[24]+e[7]*e[23]*e[17]+e[7]*e[14]*e[26]+e[7]*e[22]*e[16]+e[7]*e[13]*e[25]+e[1]*e[18]*e[12]+e[1]*e[9]*e[21];
+        A[107]=e[10]*e[27]*e[12]+e[10]*e[9]*e[30]+e[10]*e[29]*e[14]+e[10]*e[11]*e[32]+e[10]*e[28]*e[13]+e[28]*e[11]*e[14]+e[28]*e[9]*e[12]+e[13]*e[30]*e[12]+e[13]*e[32]*e[14]+e[16]*e[30]*e[15]+e[16]*e[12]*e[33]+e[16]*e[32]*e[17]+e[16]*e[14]*e[35]+e[16]*e[13]*e[34]+e[34]*e[14]*e[17]+e[34]*e[12]*e[15]-1.*e[13]*e[27]*e[9]-1.*e[13]*e[33]*e[15]-1.*e[13]*e[35]*e[17]-1.*e[13]*e[29]*e[11]+.5000000000*ep2[10]*e[31]+.5000000000*e[31]*ep2[16]-.5000000000*e[31]*ep2[9]-.5000000000*e[31]*ep2[11]+.5000000000*e[31]*ep2[12]-.5000000000*e[31]*ep2[15]-.5000000000*e[31]*ep2[17]+.5000000000*e[31]*ep2[14]+1.500000000*e[31]*ep2[13];
+        A[106]=-.5000000000*e[4]*ep2[6]-.5000000000*e[4]*ep2[0]+e[1]*e[2]*e[5]+.5000000000*e[4]*ep2[7]+e[1]*e[0]*e[3]+e[7]*e[5]*e[8]-.5000000000*e[4]*ep2[8]+.5000000000*e[4]*ep2[3]+.5000000000*e[4]*ep2[5]+e[7]*e[3]*e[6]-.5000000000*e[4]*ep2[2]+.5000000000*ep3[4]+.5000000000*ep2[1]*e[4];
+        A[100]=e[34]*e[32]*e[35]-.5000000000*e[31]*ep2[35]+.5000000000*e[31]*ep2[34]+.5000000000*ep2[28]*e[31]+.5000000000*ep3[31]+.5000000000*e[31]*ep2[32]+e[34]*e[30]*e[33]-.5000000000*e[31]*ep2[27]+.5000000000*e[31]*ep2[30]-.5000000000*e[31]*ep2[33]-.5000000000*e[31]*ep2[29]+e[28]*e[29]*e[32]+e[28]*e[27]*e[30];
+        A[101]=e[1]*e[27]*e[30]+e[1]*e[29]*e[32]+e[1]*e[28]*e[31]+e[31]*e[30]*e[3]+e[31]*e[32]*e[5]+e[7]*e[30]*e[33]+e[7]*e[32]*e[35]+e[7]*e[31]*e[34]+e[28]*e[27]*e[3]+e[28]*e[0]*e[30]+e[28]*e[29]*e[5]+e[28]*e[2]*e[32]+e[34]*e[30]*e[6]+e[34]*e[3]*e[33]+e[34]*e[32]*e[8]+e[34]*e[5]*e[35]-1.*e[31]*e[27]*e[0]-1.*e[31]*e[33]*e[6]-1.*e[31]*e[35]*e[8]-1.*e[31]*e[29]*e[2]+.5000000000*e[4]*ep2[30]+.5000000000*e[4]*ep2[32]+1.500000000*e[4]*ep2[31]-.5000000000*e[4]*ep2[27]+.5000000000*e[4]*ep2[28]-.5000000000*e[4]*ep2[29]-.5000000000*e[4]*ep2[33]+.5000000000*e[4]*ep2[34]-.5000000000*e[4]*ep2[35];
+        A[102]=.5000000000*e[22]*ep2[30]+.5000000000*e[22]*ep2[32]+1.500000000*e[22]*ep2[31]+.5000000000*e[22]*ep2[34]-.5000000000*e[22]*ep2[27]-.5000000000*e[22]*ep2[29]-.5000000000*e[22]*ep2[33]-.5000000000*e[22]*ep2[35]+e[28]*e[18]*e[30]+e[28]*e[29]*e[23]+e[28]*e[20]*e[32]+e[31]*e[30]*e[21]+e[31]*e[32]*e[23]+e[25]*e[30]*e[33]+e[25]*e[32]*e[35]+e[25]*e[31]*e[34]+e[34]*e[30]*e[24]+e[34]*e[21]*e[33]+e[34]*e[32]*e[26]+e[34]*e[23]*e[35]-1.*e[31]*e[27]*e[18]-1.*e[31]*e[33]*e[24]-1.*e[31]*e[29]*e[20]-1.*e[31]*e[35]*e[26]+e[19]*e[27]*e[30]+e[19]*e[29]*e[32]+e[19]*e[28]*e[31]+e[28]*e[27]*e[21]+.5000000000*ep2[28]*e[22];
+        A[103]=e[16]*e[30]*e[33]+e[16]*e[32]*e[35]+e[10]*e[27]*e[30]+e[10]*e[29]*e[32]+e[10]*e[28]*e[31]+e[34]*e[30]*e[15]+e[34]*e[12]*e[33]+e[34]*e[32]*e[17]+e[34]*e[14]*e[35]+e[34]*e[31]*e[16]+e[28]*e[27]*e[12]+e[28]*e[9]*e[30]+e[28]*e[29]*e[14]+e[28]*e[11]*e[32]-1.*e[31]*e[27]*e[9]+e[31]*e[30]*e[12]+e[31]*e[32]*e[14]-1.*e[31]*e[33]*e[15]-1.*e[31]*e[35]*e[17]-1.*e[31]*e[29]*e[11]-.5000000000*e[13]*ep2[27]+.5000000000*e[13]*ep2[32]+.5000000000*e[13]*ep2[28]-.5000000000*e[13]*ep2[29]+1.500000000*e[13]*ep2[31]-.5000000000*e[13]*ep2[33]+.5000000000*e[13]*ep2[30]+.5000000000*e[13]*ep2[34]-.5000000000*e[13]*ep2[35];
+        A[96]=e[21]*e[23]*e[14]+e[21]*e[22]*e[13]+e[24]*e[21]*e[15]+e[24]*e[23]*e[17]+e[24]*e[14]*e[26]+e[24]*e[22]*e[16]+e[24]*e[13]*e[25]+e[15]*e[22]*e[25]+e[15]*e[23]*e[26]+e[9]*e[19]*e[22]+e[9]*e[18]*e[21]+e[9]*e[20]*e[23]+e[18]*e[20]*e[14]+e[18]*e[11]*e[23]+e[18]*e[19]*e[13]+e[18]*e[10]*e[22]-1.*e[21]*e[25]*e[16]-1.*e[21]*e[26]*e[17]-1.*e[21]*e[20]*e[11]-1.*e[21]*e[19]*e[10]+1.500000000*ep2[21]*e[12]+.5000000000*e[12]*ep2[24]-.5000000000*e[12]*ep2[26]+.5000000000*e[12]*ep2[18]+.5000000000*e[12]*ep2[23]-.5000000000*e[12]*ep2[19]-.5000000000*e[12]*ep2[20]+.5000000000*e[12]*ep2[22]-.5000000000*e[12]*ep2[25];
+        A[97]=-1.*e[12]*e[29]*e[20]-1.*e[12]*e[35]*e[26]-1.*e[12]*e[28]*e[19]-1.*e[12]*e[34]*e[25]+e[18]*e[29]*e[14]+e[18]*e[11]*e[32]+e[18]*e[28]*e[13]+e[18]*e[10]*e[31]+e[27]*e[20]*e[14]+e[27]*e[11]*e[23]+e[27]*e[19]*e[13]+e[27]*e[10]*e[22]+e[15]*e[30]*e[24]+e[15]*e[21]*e[33]+e[15]*e[31]*e[25]+e[15]*e[22]*e[34]+e[15]*e[32]*e[26]+e[15]*e[23]*e[35]-1.*e[21]*e[28]*e[10]-1.*e[21]*e[34]*e[16]-1.*e[21]*e[35]*e[17]-1.*e[21]*e[29]*e[11]-1.*e[30]*e[25]*e[16]-1.*e[30]*e[26]*e[17]-1.*e[30]*e[20]*e[11]-1.*e[30]*e[19]*e[10]+e[24]*e[32]*e[17]+e[24]*e[14]*e[35]+e[24]*e[31]*e[16]+e[24]*e[13]*e[34]+e[33]*e[23]*e[17]+e[33]*e[14]*e[26]+e[33]*e[22]*e[16]+e[33]*e[13]*e[25]+3.*e[12]*e[30]*e[21]+e[12]*e[31]*e[22]+e[12]*e[32]*e[23]+e[9]*e[27]*e[21]+e[9]*e[18]*e[30]+e[9]*e[28]*e[22]+e[9]*e[19]*e[31]+e[9]*e[29]*e[23]+e[9]*e[20]*e[32]+e[21]*e[32]*e[14]+e[21]*e[31]*e[13]+e[30]*e[23]*e[14]+e[30]*e[22]*e[13]+e[12]*e[27]*e[18]+e[12]*e[33]*e[24];
+        A[98]=e[0]*e[11]*e[5]+e[0]*e[2]*e[14]+e[9]*e[1]*e[4]+e[9]*e[0]*e[3]+e[9]*e[2]*e[5]+e[3]*e[13]*e[4]+e[3]*e[14]*e[5]+e[6]*e[3]*e[15]+e[6]*e[13]*e[7]+e[6]*e[4]*e[16]+e[6]*e[14]*e[8]+e[6]*e[5]*e[17]+e[15]*e[4]*e[7]+e[15]*e[5]*e[8]-1.*e[3]*e[11]*e[2]-1.*e[3]*e[10]*e[1]-1.*e[3]*e[16]*e[7]-1.*e[3]*e[17]*e[8]+e[0]*e[10]*e[4]+e[0]*e[1]*e[13]+1.500000000*e[12]*ep2[3]+.5000000000*e[12]*ep2[4]+.5000000000*e[12]*ep2[5]+.5000000000*e[12]*ep2[6]+.5000000000*ep2[0]*e[12]-.5000000000*e[12]*ep2[1]-.5000000000*e[12]*ep2[7]-.5000000000*e[12]*ep2[2]-.5000000000*e[12]*ep2[8];
+        A[99]=e[21]*e[24]*e[6]+e[0]*e[19]*e[22]+e[0]*e[20]*e[23]+e[24]*e[22]*e[7]+e[24]*e[4]*e[25]+e[24]*e[23]*e[8]+e[24]*e[5]*e[26]+e[6]*e[22]*e[25]+e[6]*e[23]*e[26]+e[18]*e[0]*e[21]+e[18]*e[19]*e[4]+e[18]*e[1]*e[22]+e[18]*e[20]*e[5]+e[18]*e[2]*e[23]+e[21]*e[22]*e[4]+e[21]*e[23]*e[5]-1.*e[21]*e[26]*e[8]-1.*e[21]*e[20]*e[2]-1.*e[21]*e[19]*e[1]-1.*e[21]*e[25]*e[7]+1.500000000*ep2[21]*e[3]+.5000000000*e[3]*ep2[22]+.5000000000*e[3]*ep2[23]+.5000000000*e[3]*ep2[24]-.5000000000*e[3]*ep2[26]-.5000000000*e[3]*ep2[19]-.5000000000*e[3]*ep2[20]-.5000000000*e[3]*ep2[25]+.5000000000*ep2[18]*e[3];
+        A[127]=e[11]*e[27]*e[12]+e[11]*e[9]*e[30]+e[11]*e[29]*e[14]+e[11]*e[28]*e[13]+e[11]*e[10]*e[31]+e[29]*e[9]*e[12]+e[29]*e[10]*e[13]+e[14]*e[30]*e[12]+e[14]*e[31]*e[13]+e[17]*e[30]*e[15]+e[17]*e[12]*e[33]+e[17]*e[14]*e[35]+e[17]*e[31]*e[16]+e[17]*e[13]*e[34]+e[35]*e[12]*e[15]+e[35]*e[13]*e[16]-1.*e[14]*e[27]*e[9]-1.*e[14]*e[28]*e[10]-1.*e[14]*e[33]*e[15]-1.*e[14]*e[34]*e[16]+.5000000000*ep2[11]*e[32]-.5000000000*e[32]*ep2[16]-.5000000000*e[32]*ep2[9]+.5000000000*e[32]*ep2[12]-.5000000000*e[32]*ep2[15]+.5000000000*e[32]*ep2[17]-.5000000000*e[32]*ep2[10]+1.500000000*e[32]*ep2[14]+.5000000000*e[32]*ep2[13];
+        A[126]=e[8]*e[3]*e[6]+.5000000000*ep2[2]*e[5]-.5000000000*e[5]*ep2[0]+.5000000000*e[5]*ep2[4]-.5000000000*e[5]*ep2[6]+.5000000000*e[5]*ep2[8]+e[8]*e[4]*e[7]+.5000000000*ep3[5]+e[2]*e[0]*e[3]+.5000000000*e[5]*ep2[3]-.5000000000*e[5]*ep2[7]+e[2]*e[1]*e[4]-.5000000000*e[5]*ep2[1];
+        A[125]=e[2]*e[27]*e[3]+e[2]*e[0]*e[30]+e[2]*e[28]*e[4]+e[2]*e[1]*e[31]+e[2]*e[29]*e[5]-1.*e[5]*e[27]*e[0]-1.*e[5]*e[34]*e[7]-1.*e[5]*e[33]*e[6]+e[5]*e[30]*e[3]+e[5]*e[35]*e[8]-1.*e[5]*e[28]*e[1]+e[5]*e[31]*e[4]+e[29]*e[1]*e[4]+e[29]*e[0]*e[3]+e[8]*e[30]*e[6]+e[8]*e[3]*e[33]+e[8]*e[31]*e[7]+e[8]*e[4]*e[34]+e[35]*e[4]*e[7]+e[35]*e[3]*e[6]+.5000000000*ep2[2]*e[32]+1.500000000*e[32]*ep2[5]+.5000000000*e[32]*ep2[4]-.5000000000*e[32]*ep2[0]-.5000000000*e[32]*ep2[6]-.5000000000*e[32]*ep2[1]-.5000000000*e[32]*ep2[7]+.5000000000*e[32]*ep2[3]+.5000000000*e[32]*ep2[8];
+        A[124]=-1.*e[14]*e[19]*e[1]+e[14]*e[22]*e[4]-1.*e[14]*e[18]*e[0]-1.*e[14]*e[25]*e[7]-1.*e[14]*e[24]*e[6]-1.*e[23]*e[10]*e[1]+e[23]*e[13]*e[4]-1.*e[23]*e[16]*e[7]-1.*e[23]*e[15]*e[6]-1.*e[23]*e[9]*e[0]+e[23]*e[12]*e[3]+e[17]*e[21]*e[6]+e[17]*e[3]*e[24]+e[17]*e[22]*e[7]+e[17]*e[4]*e[25]+e[17]*e[5]*e[26]-1.*e[5]*e[24]*e[15]-1.*e[5]*e[25]*e[16]-1.*e[5]*e[18]*e[9]-1.*e[5]*e[19]*e[10]+e[26]*e[12]*e[6]+e[26]*e[3]*e[15]+e[26]*e[13]*e[7]+e[26]*e[4]*e[16]+e[11]*e[18]*e[3]+e[11]*e[0]*e[21]+e[11]*e[19]*e[4]+e[11]*e[1]*e[22]+e[11]*e[20]*e[5]+e[11]*e[2]*e[23]+e[20]*e[9]*e[3]+e[20]*e[0]*e[12]+e[20]*e[10]*e[4]+e[20]*e[1]*e[13]+e[20]*e[2]*e[14]+e[5]*e[21]*e[12]+3.*e[5]*e[23]*e[14]+e[5]*e[22]*e[13]+e[8]*e[21]*e[15]+e[8]*e[12]*e[24]+e[8]*e[23]*e[17]+e[8]*e[14]*e[26]+e[8]*e[22]*e[16]+e[8]*e[13]*e[25]+e[2]*e[18]*e[12]+e[2]*e[9]*e[21]+e[2]*e[19]*e[13]+e[2]*e[10]*e[22]+e[14]*e[21]*e[3];
+        A[123]=-.5000000000*e[14]*ep2[27]+1.500000000*e[14]*ep2[32]-.5000000000*e[14]*ep2[28]+.5000000000*e[14]*ep2[29]+.5000000000*e[14]*ep2[31]-.5000000000*e[14]*ep2[33]+.5000000000*e[14]*ep2[30]-.5000000000*e[14]*ep2[34]+.5000000000*e[14]*ep2[35]+e[11]*e[27]*e[30]+e[11]*e[29]*e[32]+e[11]*e[28]*e[31]+e[35]*e[30]*e[15]+e[35]*e[12]*e[33]+e[35]*e[32]*e[17]+e[35]*e[31]*e[16]+e[35]*e[13]*e[34]+e[29]*e[27]*e[12]+e[29]*e[9]*e[30]+e[29]*e[28]*e[13]+e[29]*e[10]*e[31]-1.*e[32]*e[27]*e[9]+e[32]*e[30]*e[12]-1.*e[32]*e[28]*e[10]+e[32]*e[31]*e[13]-1.*e[32]*e[33]*e[15]-1.*e[32]*e[34]*e[16]+e[17]*e[30]*e[33]+e[17]*e[31]*e[34];
+        A[122]=-.5000000000*e[23]*ep2[33]-.5000000000*e[23]*ep2[34]+.5000000000*ep2[29]*e[23]+.5000000000*e[23]*ep2[30]+1.500000000*e[23]*ep2[32]+.5000000000*e[23]*ep2[31]+.5000000000*e[23]*ep2[35]-.5000000000*e[23]*ep2[27]-.5000000000*e[23]*ep2[28]+e[32]*e[30]*e[21]+e[32]*e[31]*e[22]+e[26]*e[30]*e[33]+e[26]*e[32]*e[35]+e[26]*e[31]*e[34]+e[35]*e[30]*e[24]+e[35]*e[21]*e[33]+e[35]*e[31]*e[25]+e[35]*e[22]*e[34]-1.*e[32]*e[27]*e[18]-1.*e[32]*e[33]*e[24]-1.*e[32]*e[28]*e[19]-1.*e[32]*e[34]*e[25]+e[20]*e[27]*e[30]+e[20]*e[29]*e[32]+e[20]*e[28]*e[31]+e[29]*e[27]*e[21]+e[29]*e[18]*e[30]+e[29]*e[28]*e[22]+e[29]*e[19]*e[31];
+        A[121]=e[2]*e[27]*e[30]+e[2]*e[29]*e[32]+e[2]*e[28]*e[31]+e[32]*e[30]*e[3]+e[32]*e[31]*e[4]+e[8]*e[30]*e[33]+e[8]*e[32]*e[35]+e[8]*e[31]*e[34]+e[29]*e[27]*e[3]+e[29]*e[0]*e[30]+e[29]*e[28]*e[4]+e[29]*e[1]*e[31]+e[35]*e[30]*e[6]+e[35]*e[3]*e[33]+e[35]*e[31]*e[7]+e[35]*e[4]*e[34]-1.*e[32]*e[27]*e[0]-1.*e[32]*e[34]*e[7]-1.*e[32]*e[33]*e[6]-1.*e[32]*e[28]*e[1]+.5000000000*e[5]*ep2[30]+1.500000000*e[5]*ep2[32]+.5000000000*e[5]*ep2[31]-.5000000000*e[5]*ep2[27]-.5000000000*e[5]*ep2[28]+.5000000000*e[5]*ep2[29]-.5000000000*e[5]*ep2[33]-.5000000000*e[5]*ep2[34]+.5000000000*e[5]*ep2[35];
+        A[120]=.5000000000*e[32]*ep2[31]+.5000000000*e[32]*ep2[35]-.5000000000*e[32]*ep2[27]+e[29]*e[27]*e[30]+e[29]*e[28]*e[31]+e[35]*e[30]*e[33]+e[35]*e[31]*e[34]+.5000000000*ep2[29]*e[32]+.5000000000*ep3[32]-.5000000000*e[32]*ep2[33]-.5000000000*e[32]*ep2[34]+.5000000000*e[32]*ep2[30]-.5000000000*e[32]*ep2[28];
+        A[118]=e[10]*e[1]*e[4]+e[10]*e[0]*e[3]+e[10]*e[2]*e[5]+e[4]*e[12]*e[3]+e[4]*e[14]*e[5]+e[7]*e[12]*e[6]+e[7]*e[3]*e[15]+e[7]*e[4]*e[16]+e[7]*e[14]*e[8]+e[7]*e[5]*e[17]+e[16]*e[3]*e[6]+e[16]*e[5]*e[8]-1.*e[4]*e[11]*e[2]-1.*e[4]*e[15]*e[6]-1.*e[4]*e[9]*e[0]-1.*e[4]*e[17]*e[8]+e[1]*e[9]*e[3]+e[1]*e[0]*e[12]+e[1]*e[11]*e[5]+e[1]*e[2]*e[14]+1.500000000*e[13]*ep2[4]+.5000000000*e[13]*ep2[3]+.5000000000*e[13]*ep2[5]+.5000000000*e[13]*ep2[7]+.5000000000*ep2[1]*e[13]-.5000000000*e[13]*ep2[0]-.5000000000*e[13]*ep2[6]-.5000000000*e[13]*ep2[2]-.5000000000*e[13]*ep2[8];
+        A[119]=e[25]*e[21]*e[6]+e[25]*e[3]*e[24]+e[25]*e[23]*e[8]+e[25]*e[5]*e[26]+e[7]*e[21]*e[24]+e[7]*e[23]*e[26]+e[19]*e[18]*e[3]+e[19]*e[0]*e[21]+e[19]*e[1]*e[22]+e[19]*e[20]*e[5]+e[19]*e[2]*e[23]+e[22]*e[21]*e[3]+e[22]*e[23]*e[5]-1.*e[22]*e[26]*e[8]-1.*e[22]*e[20]*e[2]-1.*e[22]*e[18]*e[0]+e[22]*e[25]*e[7]-1.*e[22]*e[24]*e[6]+e[1]*e[18]*e[21]+e[1]*e[20]*e[23]+.5000000000*e[4]*ep2[25]-.5000000000*e[4]*ep2[26]-.5000000000*e[4]*ep2[18]-.5000000000*e[4]*ep2[20]-.5000000000*e[4]*ep2[24]+.5000000000*ep2[19]*e[4]+1.500000000*ep2[22]*e[4]+.5000000000*e[4]*ep2[21]+.5000000000*e[4]*ep2[23];
+        A[116]=e[22]*e[21]*e[12]+e[22]*e[23]*e[14]+e[25]*e[21]*e[15]+e[25]*e[12]*e[24]+e[25]*e[23]*e[17]+e[25]*e[14]*e[26]+e[25]*e[22]*e[16]+e[16]*e[21]*e[24]+e[16]*e[23]*e[26]+e[10]*e[19]*e[22]+e[10]*e[18]*e[21]+e[10]*e[20]*e[23]+e[19]*e[18]*e[12]+e[19]*e[9]*e[21]+e[19]*e[20]*e[14]+e[19]*e[11]*e[23]-1.*e[22]*e[24]*e[15]-1.*e[22]*e[26]*e[17]-1.*e[22]*e[20]*e[11]-1.*e[22]*e[18]*e[9]-.5000000000*e[13]*ep2[26]-.5000000000*e[13]*ep2[18]+.5000000000*e[13]*ep2[23]+.5000000000*e[13]*ep2[19]-.5000000000*e[13]*ep2[20]-.5000000000*e[13]*ep2[24]+.5000000000*e[13]*ep2[21]+1.500000000*ep2[22]*e[13]+.5000000000*e[13]*ep2[25];
+        A[117]=e[13]*e[30]*e[21]+3.*e[13]*e[31]*e[22]+e[13]*e[32]*e[23]+e[10]*e[27]*e[21]+e[10]*e[18]*e[30]+e[10]*e[28]*e[22]+e[10]*e[19]*e[31]+e[10]*e[29]*e[23]+e[10]*e[20]*e[32]+e[22]*e[30]*e[12]+e[22]*e[32]*e[14]+e[31]*e[21]*e[12]+e[31]*e[23]*e[14]-1.*e[13]*e[27]*e[18]-1.*e[13]*e[33]*e[24]-1.*e[13]*e[29]*e[20]-1.*e[13]*e[35]*e[26]+e[13]*e[28]*e[19]+e[13]*e[34]*e[25]+e[19]*e[27]*e[12]+e[19]*e[9]*e[30]+e[19]*e[29]*e[14]+e[19]*e[11]*e[32]+e[28]*e[18]*e[12]+e[28]*e[9]*e[21]+e[28]*e[20]*e[14]+e[28]*e[11]*e[23]+e[16]*e[30]*e[24]+e[16]*e[21]*e[33]+e[16]*e[31]*e[25]+e[16]*e[22]*e[34]+e[16]*e[32]*e[26]+e[16]*e[23]*e[35]-1.*e[22]*e[27]*e[9]-1.*e[22]*e[33]*e[15]-1.*e[22]*e[35]*e[17]-1.*e[22]*e[29]*e[11]-1.*e[31]*e[24]*e[15]-1.*e[31]*e[26]*e[17]-1.*e[31]*e[20]*e[11]-1.*e[31]*e[18]*e[9]+e[25]*e[30]*e[15]+e[25]*e[12]*e[33]+e[25]*e[32]*e[17]+e[25]*e[14]*e[35]+e[34]*e[21]*e[15]+e[34]*e[12]*e[24]+e[34]*e[23]*e[17]+e[34]*e[14]*e[26];
+        A[114]=e[19]*e[11]*e[14]+e[19]*e[9]*e[12]+e[19]*e[10]*e[13]+e[13]*e[21]*e[12]+e[13]*e[23]*e[14]+e[16]*e[21]*e[15]+e[16]*e[12]*e[24]+e[16]*e[23]*e[17]+e[16]*e[14]*e[26]+e[16]*e[13]*e[25]+e[25]*e[14]*e[17]+e[25]*e[12]*e[15]-1.*e[13]*e[24]*e[15]-1.*e[13]*e[26]*e[17]-1.*e[13]*e[20]*e[11]-1.*e[13]*e[18]*e[9]+e[10]*e[18]*e[12]+e[10]*e[9]*e[21]+e[10]*e[20]*e[14]+e[10]*e[11]*e[23]+1.500000000*e[22]*ep2[13]+.5000000000*e[22]*ep2[14]+.5000000000*e[22]*ep2[12]+.5000000000*e[22]*ep2[16]+.5000000000*ep2[10]*e[22]-.5000000000*e[22]*ep2[9]-.5000000000*e[22]*ep2[11]-.5000000000*e[22]*ep2[15]-.5000000000*e[22]*ep2[17];
+        A[115]=e[13]*e[12]*e[3]+e[13]*e[14]*e[5]+e[16]*e[12]*e[6]+e[16]*e[3]*e[15]+e[16]*e[13]*e[7]+e[16]*e[14]*e[8]+e[16]*e[5]*e[17]+e[7]*e[14]*e[17]+e[7]*e[12]*e[15]+e[1]*e[11]*e[14]+e[1]*e[9]*e[12]+e[1]*e[10]*e[13]+e[10]*e[9]*e[3]+e[10]*e[0]*e[12]+e[10]*e[11]*e[5]+e[10]*e[2]*e[14]-1.*e[13]*e[11]*e[2]-1.*e[13]*e[15]*e[6]-1.*e[13]*e[9]*e[0]-1.*e[13]*e[17]*e[8]+1.500000000*ep2[13]*e[4]+.5000000000*e[4]*ep2[16]-.5000000000*e[4]*ep2[9]-.5000000000*e[4]*ep2[11]+.5000000000*e[4]*ep2[12]-.5000000000*e[4]*ep2[15]-.5000000000*e[4]*ep2[17]+.5000000000*e[4]*ep2[10]+.5000000000*e[4]*ep2[14];
+        A[112]=e[19]*e[1]*e[4]+e[19]*e[0]*e[3]+e[19]*e[2]*e[5]+e[4]*e[21]*e[3]+e[4]*e[23]*e[5]+e[7]*e[21]*e[6]+e[7]*e[3]*e[24]+e[7]*e[4]*e[25]+e[7]*e[23]*e[8]+e[7]*e[5]*e[26]+e[25]*e[3]*e[6]+e[25]*e[5]*e[8]+e[1]*e[18]*e[3]+e[1]*e[0]*e[21]+e[1]*e[20]*e[5]+e[1]*e[2]*e[23]-1.*e[4]*e[26]*e[8]-1.*e[4]*e[20]*e[2]-1.*e[4]*e[18]*e[0]-1.*e[4]*e[24]*e[6]+1.500000000*e[22]*ep2[4]-.5000000000*e[22]*ep2[0]-.5000000000*e[22]*ep2[6]+.5000000000*e[22]*ep2[5]+.5000000000*e[22]*ep2[1]+.5000000000*e[22]*ep2[7]+.5000000000*e[22]*ep2[3]-.5000000000*e[22]*ep2[2]-.5000000000*e[22]*ep2[8];
+        A[113]=-1.*e[31]*e[20]*e[2]-1.*e[31]*e[18]*e[0]+e[31]*e[23]*e[5]-1.*e[31]*e[24]*e[6]+e[7]*e[30]*e[24]+e[7]*e[21]*e[33]+e[7]*e[32]*e[26]+e[7]*e[23]*e[35]+e[25]*e[30]*e[6]+e[25]*e[3]*e[33]+e[25]*e[31]*e[7]+e[25]*e[4]*e[34]+e[25]*e[32]*e[8]+e[25]*e[5]*e[35]+e[34]*e[21]*e[6]+e[34]*e[3]*e[24]+e[34]*e[22]*e[7]+e[34]*e[23]*e[8]+e[34]*e[5]*e[26]+e[1]*e[27]*e[21]+e[1]*e[18]*e[30]+e[1]*e[28]*e[22]+e[1]*e[19]*e[31]+e[1]*e[29]*e[23]+e[1]*e[20]*e[32]+e[19]*e[27]*e[3]+e[19]*e[0]*e[30]+e[19]*e[28]*e[4]+e[19]*e[29]*e[5]+e[19]*e[2]*e[32]+e[28]*e[18]*e[3]+e[28]*e[0]*e[21]+e[28]*e[20]*e[5]+e[28]*e[2]*e[23]+e[4]*e[30]*e[21]+3.*e[4]*e[31]*e[22]+e[4]*e[32]*e[23]-1.*e[4]*e[27]*e[18]-1.*e[4]*e[33]*e[24]-1.*e[4]*e[29]*e[20]-1.*e[4]*e[35]*e[26]-1.*e[22]*e[27]*e[0]+e[22]*e[32]*e[5]-1.*e[22]*e[33]*e[6]+e[22]*e[30]*e[3]-1.*e[22]*e[35]*e[8]-1.*e[22]*e[29]*e[2]+e[31]*e[21]*e[3]-1.*e[31]*e[26]*e[8];
+
+        int perm[20] = {6, 8, 18, 15, 12, 5, 14, 7, 4, 11, 19, 13, 1, 16, 17, 3, 10, 9, 2, 0};
+        double AA[200];
+        for (int i = 0; i < 20; i++)
+        {
+            for (int j = 0; j < 10; j++) AA[i + j * 20] = A[perm[i] + j * 20];
+        }
+
+        for (int i = 0; i < 200; i++)
+        {
+            A[i] = AA[i];
+        }
+    }
+
+
+    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
+    {
+        Mat X1 = _m1.getMat(), X2 = _m2.getMat(), model = _model.getMat();
+        const Point2d* x1ptr = X1.ptr<Point2d>();
+        const Point2d* x2ptr = X2.ptr<Point2d>();
+        int n = X1.checkVector(2);
+        Matx33d E(model.ptr<double>());
+
+        _err.create(n, 1, CV_32F);
+        Mat err = _err.getMat();
+
+        for (int i = 0; i < n; i++)
+        {
+            Vec3d x1(x1ptr[i].x, x1ptr[i].y, 1.);
+            Vec3d x2(x2ptr[i].x, x2ptr[i].y, 1.);
+            Vec3d Ex1 = E * x1;
+            Vec3d Etx2 = E.t() * x2;
+            double x2tEx1 = x2.dot(Ex1);
+
+            double a = Ex1[0] * Ex1[0];
+            double b = Ex1[1] * Ex1[1];
+            double c = Etx2[0] * Etx2[0];
+            double d = Etx2[1] * Etx2[1];
+
+            err.at<float>(i) = (float)(x2tEx1 * x2tEx1 / (a + b + c + d));
+        }
+    }
+};
+
+// Input should be a vector of n 2D points or a Nx2 matrix
+cv::Mat findEssentialMat( InputArray _points1, InputArray _points2, InputArray _cameraMatrix,
+                              int method, double prob, double threshold, OutputArray _mask)
+{
+    Mat points1, points2, cameraMatrix;
+    _points1.getMat().convertTo(points1, CV_64F);
+    _points2.getMat().convertTo(points2, CV_64F);
+    _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);
+
+    int npoints = points1.checkVector(2);
+    CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
+                              points1.type() == points2.type());
+
+    CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1);
+
+    if (points1.channels() > 1)
+    {
+        points1 = points1.reshape(1, npoints);
+        points2 = points2.reshape(1, npoints);
+    }
+
+    double fx = cameraMatrix.at<double>(0,0);
+    double fy = cameraMatrix.at<double>(1,1);
+    double cx = cameraMatrix.at<double>(0,2);
+    double cy = cameraMatrix.at<double>(1,2);
+
+    points1.col(0) = (points1.col(0) - cx) / fx;
+    points2.col(0) = (points2.col(0) - cx) / fx;
+    points1.col(1) = (points1.col(1) - cy) / fy;
+    points2.col(1) = (points2.col(1) - cy) / fy;
+
+    // Reshape data to fit opencv ransac function
+    points1 = points1.reshape(2, npoints);
+    points2 = points2.reshape(2, npoints);
+
+    threshold /= (fx+fy)/2;
+
+    Mat E;
+    Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
+    cb = Ptr<EMEstimatorCallback>(new EMEstimatorCallback());
+
+    if( method == RANSAC )
+    	createRANSACPointSetRegistrator(cb, 6, threshold, prob)->run(points1, points2, E, _mask);
+    else
+        createLMeDSPointSetRegistrator(cb, 6, prob)->run(points1, points2, E, _mask);
+
+    return E;
+}
+
+void decomposeEssentialMat( InputArray _E, OutputArray _R1, OutputArray _R2, OutputArray _t )
+{
+    Mat E = _E.getMat().reshape(1, 3);
+    CV_Assert(E.cols == 3 && E.rows == 3);
+
+    Mat D, U, Vt;
+    SVD::compute(E, D, U, Vt);
+
+    if (determinant(U) < 0) U *= -1.;
+    if (determinant(Vt) < 0) Vt *= -1.;
+
+    Mat W = (Mat_<double>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);
+    W.convertTo(W, E.type());
+
+    Mat R1, R2, t;
+    R1 = U * W * Vt;
+    R2 = U * W.t() * Vt;
+    t = U.col(2) * 1.0;
+
+    R1.copyTo(_R1);
+    R2.copyTo(_R2);
+    t.copyTo(_t);
+}
+
+int recoverPose( InputArray E, InputArray _points1, InputArray _points2,
+                 InputArray _cameraMatrix, OutputArray _R, OutputArray _t, double distanceThresh,
+                 InputOutputArray _mask, OutputArray triangulatedPoints)
+{
+    Mat points1, points2, cameraMatrix;
+    _points1.getMat().convertTo(points1, CV_64F);
+    _points2.getMat().convertTo(points2, CV_64F);
+    _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);
+
+    int npoints = points1.checkVector(2);
+    CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
+                              points1.type() == points2.type());
+
+    CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1);
+
+    if (points1.channels() > 1)
+    {
+        points1 = points1.reshape(1, npoints);
+        points2 = points2.reshape(1, npoints);
+    }
+
+    double fx = cameraMatrix.at<double>(0,0);
+    double fy = cameraMatrix.at<double>(1,1);
+    double cx = cameraMatrix.at<double>(0,2);
+    double cy = cameraMatrix.at<double>(1,2);
+
+    points1.col(0) = (points1.col(0) - cx) / fx;
+    points2.col(0) = (points2.col(0) - cx) / fx;
+    points1.col(1) = (points1.col(1) - cy) / fy;
+    points2.col(1) = (points2.col(1) - cy) / fy;
+
+    points1 = points1.t();
+    points2 = points2.t();
+
+    Mat R1, R2, t;
+    cv3::decomposeEssentialMat(E, R1, R2, t);
+    Mat P0 = Mat::eye(3, 4, R1.type());
+    Mat P1(3, 4, R1.type()), P2(3, 4, R1.type()), P3(3, 4, R1.type()), P4(3, 4, R1.type());
+    P1(Range::all(), Range(0, 3)) = R1 * 1.0; P1.col(3) = t * 1.0;
+    P2(Range::all(), Range(0, 3)) = R2 * 1.0; P2.col(3) = t * 1.0;
+    P3(Range::all(), Range(0, 3)) = R1 * 1.0; P3.col(3) = -t * 1.0;
+    P4(Range::all(), Range(0, 3)) = R2 * 1.0; P4.col(3) = -t * 1.0;
+
+    // Do the cheirality check.
+    // Notice here a threshold dist is used to filter
+    // out far away points (i.e. infinite points) since
+    // their depth may vary between positive and negative.
+    std::vector<Mat> allTriangulations(4);
+    Mat Q;
+
+    triangulatePoints(P0, P1, points1, points2, Q);
+    if(triangulatedPoints.needed())
+        Q.copyTo(allTriangulations[0]);
+    Mat mask1 = Q.row(2).mul(Q.row(3)) > 0;
+    Q.row(0) /= Q.row(3);
+    Q.row(1) /= Q.row(3);
+    Q.row(2) /= Q.row(3);
+    Q.row(3) /= Q.row(3);
+    mask1 = (Q.row(2) < distanceThresh) & mask1;
+    Q = P1 * Q;
+    mask1 = (Q.row(2) > 0) & mask1;
+    mask1 = (Q.row(2) < distanceThresh) & mask1;
+
+    triangulatePoints(P0, P2, points1, points2, Q);
+    if(triangulatedPoints.needed())
+        Q.copyTo(allTriangulations[1]);
+    Mat mask2 = Q.row(2).mul(Q.row(3)) > 0;
+    Q.row(0) /= Q.row(3);
+    Q.row(1) /= Q.row(3);
+    Q.row(2) /= Q.row(3);
+    Q.row(3) /= Q.row(3);
+    mask2 = (Q.row(2) < distanceThresh) & mask2;
+    Q = P2 * Q;
+    mask2 = (Q.row(2) > 0) & mask2;
+    mask2 = (Q.row(2) < distanceThresh) & mask2;
+
+    triangulatePoints(P0, P3, points1, points2, Q);
+    if(triangulatedPoints.needed())
+        Q.copyTo(allTriangulations[2]);
+    Mat mask3 = Q.row(2).mul(Q.row(3)) > 0;
+    Q.row(0) /= Q.row(3);
+    Q.row(1) /= Q.row(3);
+    Q.row(2) /= Q.row(3);
+    Q.row(3) /= Q.row(3);
+    mask3 = (Q.row(2) < distanceThresh) & mask3;
+    Q = P3 * Q;
+    mask3 = (Q.row(2) > 0) & mask3;
+    mask3 = (Q.row(2) < distanceThresh) & mask3;
+
+    triangulatePoints(P0, P4, points1, points2, Q);
+    if(triangulatedPoints.needed())
+        Q.copyTo(allTriangulations[3]);
+    Mat mask4 = Q.row(2).mul(Q.row(3)) > 0;
+    Q.row(0) /= Q.row(3);
+    Q.row(1) /= Q.row(3);
+    Q.row(2) /= Q.row(3);
+    Q.row(3) /= Q.row(3);
+    mask4 = (Q.row(2) < distanceThresh) & mask4;
+    Q = P4 * Q;
+    mask4 = (Q.row(2) > 0) & mask4;
+    mask4 = (Q.row(2) < distanceThresh) & mask4;
+
+    mask1 = mask1.t();
+    mask2 = mask2.t();
+    mask3 = mask3.t();
+    mask4 = mask4.t();
+
+    // If _mask is given, then use it to filter outliers.
+    if (!_mask.empty())
+    {
+        Mat mask = _mask.getMat();
+        CV_Assert(npoints == mask.checkVector(1));
+        mask = mask.reshape(1, npoints);
+        bitwise_and(mask, mask1, mask1);
+        bitwise_and(mask, mask2, mask2);
+        bitwise_and(mask, mask3, mask3);
+        bitwise_and(mask, mask4, mask4);
+    }
+    if (_mask.empty() && _mask.needed())
+    {
+        _mask.create(mask1.size(), CV_8U);
+    }
+
+    CV_Assert(_R.needed() && _t.needed());
+    _R.create(3, 3, R1.type());
+    _t.create(3, 1, t.type());
+
+    int good1 = countNonZero(mask1);
+    int good2 = countNonZero(mask2);
+    int good3 = countNonZero(mask3);
+    int good4 = countNonZero(mask4);
+
+    if (good1 >= good2 && good1 >= good3 && good1 >= good4)
+    {
+        if(triangulatedPoints.needed()) allTriangulations[0].copyTo(triangulatedPoints);
+        R1.copyTo(_R);
+        t.copyTo(_t);
+        if (_mask.needed()) mask1.copyTo(_mask);
+        return good1;
+    }
+    else if (good2 >= good1 && good2 >= good3 && good2 >= good4)
+    {
+        if(triangulatedPoints.needed()) allTriangulations[1].copyTo(triangulatedPoints);
+        R2.copyTo(_R);
+        t.copyTo(_t);
+        if (_mask.needed()) mask2.copyTo(_mask);
+        return good2;
+    }
+    else if (good3 >= good1 && good3 >= good2 && good3 >= good4)
+    {
+        if(triangulatedPoints.needed()) allTriangulations[2].copyTo(triangulatedPoints);
+        t = -t;
+        R1.copyTo(_R);
+        t.copyTo(_t);
+        if (_mask.needed()) mask3.copyTo(_mask);
+        return good3;
+    }
+    else
+    {
+        if(triangulatedPoints.needed()) allTriangulations[3].copyTo(triangulatedPoints);
+        t = -t;
+        R2.copyTo(_R);
+        t.copyTo(_t);
+        if (_mask.needed()) mask4.copyTo(_mask);
+        return good4;
+    }
+}
+
+}

corelib/src/opencv/five-point.h

@@ -0,0 +1,23 @@
+/*
+ * five-point.h
+ *
+ *  Created on: May 18, 2020
+ *      Author: mathieu
+ */
+
+#ifndef CORELIB_SRC_OPENCV_FIVE_POINT_H_
+#define CORELIB_SRC_OPENCV_FIVE_POINT_H_
+
+namespace cv3
+{
+
+cv::Mat findEssentialMat( cv::InputArray _points1, cv::InputArray _points2, cv::InputArray _cameraMatrix,
+                          int method, double prob, double threshold, cv::OutputArray _mask = cv::noArray());
+
+int recoverPose( cv::InputArray E, cv::InputArray _points1, cv::InputArray _points2,
+                 cv::InputArray _cameraMatrix, cv::OutputArray _R, cv::OutputArray _t, double distanceThresh,
+                 cv::InputOutputArray _mask, cv::OutputArray triangulatedPoints);
+}
+
+
+#endif /* CORELIB_SRC_OPENCV_FIVE_POINT_H_ */

corelib/src/opencv/readme.txt

@@ -1,5 +1,7 @@
 
-These files are built only if RTAB-Map is built against OpenCV 2.
-
+Used with OpenCV 2:
  * Orb.cpp is a modified version of OpenCV2 Orb with FAST object from rtabmap (FAST with Grid adaptor).
- * solvepnp.cpp is a copy of the OpenCV3 version of solvePnPRansac.
+ 
+ Used for all OpenCV versions:
+ * solvepnp.cpp is a copy of the OpenCV3 version of solvePnPRansac.
+ * five-point.cpp is a copy of the same file in OpenCV (d2872afce0fcc84a52b5753960730595550e1b62) but with RANSAC estimator using 6 points instead of 5 points to avoid "DLT algorithm needs at least 6 points for pose estimation from 3D-2D point correspondences. (expected: 'count >= 6')" error on recent OpenCV versions using DLT by default.

corelib/src/optimizer/OptimizerG2O.cpp

@@ -197,7 +197,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		// Apply g2o optimization
 
 		g2o::SparseOptimizer optimizer;
-		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
+		//optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
 		if (isSlam2d())
 		{
 			g2o::ParameterSE2Offset* odomOffset = new g2o::ParameterSE2Offset();
@@ -1260,7 +1260,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 	if(poses.size()>=2 && iterations() > 0 && (models.size() == poses.size() || poses.begin()->first < 0))
 	{
 		g2o::SparseOptimizer optimizer;
-		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
+		//optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
 #if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM2)
 		std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
 #else

corelib/src/pymatcher/PyMatcher.cpp

@@ -0,0 +1,313 @@
+/**
+ * Python interface for SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork
+ */
+
+#include <pymatcher/PyMatcher.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#define NPY_NO_DEPRECATED_API NPY_API_VERSION
+#include <numpy/arrayobject.h>
+
+namespace rtabmap
+{
+
+class PythonSingleTon
+{
+public:
+	PythonSingleTon() : initialized_(false) {}
+	void init() {UScopeMutex lock(mutex_); if(!initialized_)Py_Initialize(); initialized_=true;}
+	bool initialized() const {return initialized_;}
+	virtual ~PythonSingleTon() {if(initialized_) Py_Finalize();}
+private:
+	bool initialized_;
+	UMutex mutex_;
+};
+
+static PythonSingleTon g_python;
+
+std::string getTraceback()
+{
+	// Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated
+
+	PyObject* type;
+	PyObject* value;
+	PyObject* traceback;
+
+	PyErr_Fetch(&type, &value, &traceback);
+	PyErr_NormalizeException(&type, &value, &traceback);
+
+	std::string fcn = "";
+	fcn += "def get_pretty_traceback(exc_type, exc_value, exc_tb):\n";
+	fcn += "    import sys, traceback\n";
+	fcn += "    lines = []\n";
+	fcn += "    lines = traceback.format_exception(exc_type, exc_value, exc_tb)\n";
+	fcn += "    output = '\\n'.join(lines)\n";
+	fcn += "    return output\n";
+
+	PyRun_SimpleString(fcn.c_str());
+	PyObject* mod = PyImport_ImportModule("__main__");
+	PyObject* method = PyObject_GetAttrString(mod, "get_pretty_traceback");
+	PyObject* outStr = PyObject_CallObject(method, Py_BuildValue("OOO", type, value, traceback));
+	std::string pretty = PyBytes_AsString(PyUnicode_AsASCIIString(outStr));
+
+	Py_DECREF(method);
+	Py_DECREF(outStr);
+	Py_DECREF(mod);
+
+	return pretty;
+}
+
+PyMatcher::PyMatcher(
+		const std::string & pythonMatcherPath,
+		float matchThreshold,
+		int iterations,
+		bool cuda,
+		const std::string & model) :
+				pModule_(0),
+				pFunc_(0),
+				matchThreshold_(matchThreshold),
+				iterations_(iterations),
+				cuda_(cuda)
+{
+	path_ = uReplaceChar(pythonMatcherPath, '~', UDirectory::homeDir());
+	model_ = uReplaceChar(model, '~', UDirectory::homeDir());
+	UINFO("path = %s", path_.c_str());
+	UINFO("model = %s", model_.c_str());
+
+	if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0)
+	{
+		UERROR("Cannot initialize Python matcher, the path is not valid: \"%s\"", path_.c_str());
+		return;
+	}
+
+	if(!g_python.initialized())
+	{
+		g_python.init();
+	}
+
+	std::string matcherPythonDir = UDirectory::getDir(path_);
+	if(!matcherPythonDir.empty())
+	{
+		PyRun_SimpleString("import sys");
+		PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
+	}
+
+	_import_array();
+
+	std::string scriptName = uSplit(UFile::getName(path_), '.').front();
+	PyObject * pName = PyUnicode_FromString(scriptName.c_str());
+	pModule_ = PyImport_Import(pName);
+	Py_DECREF(pName);
+
+	if(!pModule_)
+	{
+		UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str());
+		UERROR("%s", getTraceback().c_str());
+	}
+}
+
+PyMatcher::~PyMatcher()
+{
+	if(pFunc_)
+	{
+		Py_DECREF(pFunc_);
+	}
+	if(pModule_)
+	{
+		Py_DECREF(pModule_);
+	}
+}
+
+std::vector<cv::DMatch> PyMatcher::match(
+		  const cv::Mat & descriptorsQuery,
+		  const cv::Mat & descriptorsTrain,
+		  const std::vector<cv::KeyPoint> & keypointsQuery,
+		  const std::vector<cv::KeyPoint> & keypointsTrain,
+		  const cv::Size & imageSize)
+{
+	UTimer timer;
+	std::vector<cv::DMatch> matches;
+
+	if(!pModule_)
+	{
+		UERROR("Python matcher module not loaded!");
+		return matches;
+	}
+
+	if(!descriptorsQuery.empty() &&
+	   descriptorsQuery.cols == descriptorsTrain.cols &&
+	   descriptorsQuery.type() == CV_32F &&
+	   descriptorsTrain.type() == CV_32F &&
+	   descriptorsQuery.rows == (int)keypointsQuery.size() &&
+	   descriptorsTrain.rows == (int)keypointsTrain.size() &&
+	   imageSize.width>0 && imageSize.height>0)
+	{
+
+		UDEBUG("matchThreshold=%f, iterations=%d, cuda=%d", matchThreshold_, iterations_, cuda_?1:0);
+
+		if(!pFunc_)
+		{
+			PyObject * pFunc = PyObject_GetAttrString(pModule_, "init");
+			if(pFunc)
+			{
+				if(PyCallable_Check(pFunc))
+				{
+					PyObject * result = PyObject_CallFunction(pFunc, "ifiis", descriptorsQuery.cols, matchThreshold_, iterations_, cuda_?1:0, model_.c_str());
+
+					if(result == NULL)
+					{
+						UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str());
+						UERROR("%s", getTraceback().c_str());
+						return matches;
+					}
+					Py_DECREF(result);
+
+					pFunc_ = PyObject_GetAttrString(pModule_, "match");
+					if(pFunc_ && PyCallable_Check(pFunc_))
+					{
+						// we are ready!
+					}
+					else
+					{
+						UERROR("Cannot find method \"match(...)\" in %s", path_.c_str());
+						UERROR("%s", getTraceback().c_str());
+						if(pFunc_)
+						{
+							Py_DECREF(pFunc_);
+							pFunc_ = 0;
+						}
+						return matches;
+					}
+				}
+				else
+				{
+					UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
+					UERROR("%s", getTraceback().c_str());
+					return matches;
+				}
+				Py_DECREF(pFunc);
+			}
+			else
+			{
+				UERROR("Cannot find method \"init(...)\"");
+				UERROR("%s", getTraceback().c_str());
+				return matches;
+			}
+			UDEBUG("init time = %fs", timer.ticks());
+		}
+
+		if(pFunc_)
+		{
+			std::vector<float> descriptorsQueryV(descriptorsQuery.rows * descriptorsQuery.cols);
+			memcpy(descriptorsQueryV.data(), descriptorsQuery.data, descriptorsQuery.total()*sizeof(float));
+			npy_intp dimsFrom[2] = {descriptorsQuery.rows, descriptorsQuery.cols};
+			PyObject* pDescriptorsQuery = PyArray_SimpleNewFromData(2, dimsFrom, NPY_FLOAT, (void*)descriptorsQueryV.data());
+			UASSERT(pDescriptorsQuery);
+
+			npy_intp dimsTo[2] = {descriptorsTrain.rows, descriptorsTrain.cols};
+			std::vector<float> descriptorsTrainV(descriptorsTrain.rows * descriptorsTrain.cols);
+			memcpy(descriptorsTrainV.data(), descriptorsTrain.data, descriptorsTrain.total()*sizeof(float));
+			PyObject* pDescriptorsTrain = PyArray_SimpleNewFromData(2, dimsTo, NPY_FLOAT, (void*)descriptorsTrainV.data());
+			UASSERT(pDescriptorsTrain);
+
+			std::vector<float> keypointsQueryV(keypointsQuery.size()*2);
+			std::vector<float> scoresQuery(keypointsQuery.size());
+			for(size_t i=0; i<keypointsQuery.size(); ++i)
+			{
+				keypointsQueryV[i*2] = keypointsQuery[i].pt.x;
+				keypointsQueryV[i*2+1] = keypointsQuery[i].pt.y;
+				scoresQuery[i] = keypointsQuery[i].response;
+			}
+
+			std::vector<float> keypointsTrainV(keypointsTrain.size()*2);
+			std::vector<float> scoresTrain(keypointsTrain.size());
+			for(size_t i=0; i<keypointsTrain.size(); ++i)
+			{
+				keypointsTrainV[i*2] = keypointsTrain[i].pt.x;
+				keypointsTrainV[i*2+1] = keypointsTrain[i].pt.y;
+				scoresTrain[i] = keypointsTrain[i].response;
+			}
+
+			npy_intp dimsKpQuery[2] = {(int)keypointsQuery.size(), 2};
+			PyObject* pKeypointsQuery = PyArray_SimpleNewFromData(2, dimsKpQuery, NPY_FLOAT, (void*)keypointsQueryV.data());
+			UASSERT(pKeypointsQuery);
+
+			npy_intp dimsKpTrain[2] = {(int)keypointsTrain.size(), 2};
+			PyObject* pkeypointsTrain = PyArray_SimpleNewFromData(2, dimsKpTrain, NPY_FLOAT, (void*)keypointsTrainV.data());
+			UASSERT(pkeypointsTrain);
+
+			npy_intp dimsScoresQuery[1] = {(int)keypointsQuery.size()};
+			PyObject* pScoresQuery = PyArray_SimpleNewFromData(1, dimsScoresQuery, NPY_FLOAT, (void*)scoresQuery.data());
+			UASSERT(pScoresQuery);
+
+			npy_intp dimsScoresTrain[1] = {(int)keypointsTrain.size()};
+			PyObject* pScoresTrain = PyArray_SimpleNewFromData(1, dimsScoresTrain, NPY_FLOAT, (void*)scoresTrain.data());
+			UASSERT(pScoresTrain);
+
+			PyObject * pImageWidth = PyLong_FromLong(imageSize.width);
+			PyObject * pImageHeight = PyLong_FromLong(imageSize.height);
+
+			UDEBUG("Preparing data time = %fs", timer.ticks());
+
+			PyObject *pReturn = PyObject_CallFunctionObjArgs(pFunc_, pKeypointsQuery, pkeypointsTrain, pScoresQuery, pScoresTrain, pDescriptorsQuery, pDescriptorsTrain, pImageWidth, pImageHeight, NULL);
+			if(pReturn == NULL)
+			{
+				UERROR("Failed to call match() function!");
+				UERROR("%s", getTraceback().c_str());
+			}
+			else
+			{
+				UDEBUG("Python matching time = %fs", timer.ticks());
+
+				PyArrayObject *np_ret = reinterpret_cast<PyArrayObject*>(pReturn);
+
+				// Convert back to C++ array and print.
+				int len1 = PyArray_SHAPE(np_ret)[0];
+				int len2 = PyArray_SHAPE(np_ret)[1];
+				int type = PyArray_TYPE(np_ret);
+				UDEBUG("Matches array %dx%d (type=%d)", len1, len2, type);
+				UASSERT_MSG(type == NPY_LONG || type == NPY_INT, uFormat("Returned matches should type INT=5 or LONG=7, received type=%d", type).c_str());
+				if(type == NPY_LONG)
+				{
+					long* c_out = reinterpret_cast<long*>(PyArray_DATA(np_ret));
+					for (int i = 0; i < len1*len2; i+=2)
+					{
+						matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0));
+					}
+				}
+				else // INT
+				{
+					int* c_out = reinterpret_cast<int*>(PyArray_DATA(np_ret));
+					for (int i = 0; i < len1*len2; i+=2)
+					{
+						matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0));
+					}
+				}
+				Py_DECREF(pReturn);
+			}
+
+			Py_DECREF(pDescriptorsQuery);
+			Py_DECREF(pDescriptorsTrain);
+			Py_DECREF(pKeypointsQuery);
+			Py_DECREF(pkeypointsTrain);
+			Py_DECREF(pScoresQuery);
+			Py_DECREF(pScoresTrain);
+			Py_DECREF(pImageWidth);
+			Py_DECREF(pImageHeight);
+
+			UDEBUG("Fill matches (%d/%d) and cleanup time = %fs", matches.size(), std::min(descriptorsQuery.rows, descriptorsTrain.rows), timer.ticks());
+		}
+	}
+	else
+	{
+		UERROR("Invalid inputs! Supported python matchers require float descriptors.");
+	}
+	return matches;
+}
+
+}

corelib/src/pymatcher/PyMatcher.h

@@ -0,0 +1,54 @@
+/**
+ * Python interface for python matchers like:
+ *  - SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork
+ *  - OANET https://github.com/zjhthu/OANet
+ */
+
+#ifndef PYMATCHER_H
+#define PYMATCHER_H
+
+#include <opencv2/core/types.hpp>
+#include <opencv2/core/mat.hpp>
+#include <vector>
+
+#include <Python.h>
+
+namespace rtabmap
+{
+
+class PyMatcher
+{
+public:
+  PyMatcher(const std::string & pythonMatcherPath,
+		  float matchThreshold = 0.2f,
+		  int iterations = 20,
+		  bool cuda = true,
+		  const std::string & model = "indoor");
+  virtual ~PyMatcher();
+
+  const std::string & path() const {return path_;}
+  float matchThreshold() const {return matchThreshold_;}
+  int iterations() const {return iterations_;}
+  bool cuda() const {return cuda_;}
+  const std::string & model() const {return model_;}
+
+  std::vector<cv::DMatch> match(
+		  const cv::Mat & descriptorsQuery,
+		  const cv::Mat & descriptorsTrain,
+		  const std::vector<cv::KeyPoint> & keypointsQuery,
+		  const std::vector<cv::KeyPoint> & keypointsTrain,
+		  const cv::Size & imageSize);
+
+private:
+  PyObject * pModule_;
+  PyObject * pFunc_;
+  std::string path_;
+  float matchThreshold_;
+  int iterations_;
+  bool cuda_;
+  std::string model_;
+};
+
+}
+
+#endif

corelib/src/pymatcher/rtabmap_oanet.py

@@ -0,0 +1,44 @@
+#! /usr/bin/env python3
+#
+# Drop this file in the "demo" folder of OANet git: https://github.com/zjhthu/OANet
+# To use with rtabmap:
+#   --Vis/CorNNType 6 --PyMatcher/Path ~/OANet/demo/rtabmap_oanet.py --PyMatcher/Model ~/OANet/model/gl3d/sift-4000/model_best.pth
+#
+
+import sys    
+import os
+sys.path.append(os.path.dirname(os.path.realpath(__file__))+'/../core')
+if not hasattr(sys, 'argv'):
+    sys.argv  = ['']
+    
+#print(os.sys.path)
+#print(sys.version)
+
+import numpy as np
+from learnedmatcher import LearnedMatcher
+
+lm = None
+
+def init(descriptorDim, matchThreshold, iterations, cuda, model_path):
+    print("OANet python init()")
+    global lm
+    lm = LearnedMatcher(model_path, inlier_threshold=1, use_ratio=0, use_mutual=0)
+
+
+def match(kptsFrom, kptsTo, scoresFrom, scoresTo, descriptorsFrom, descriptorsTo, imageWidth, imageHeight):
+    #print("OANet python match()")
+    
+    kpt1 = np.asarray(kptsFrom)
+    kpt2 = np.asarray(kptsTo)
+    desc1 = np.asarray(descriptorsFrom)
+    desc2 = np.asarray(descriptorsTo)
+      
+    global lm
+    matches, _, _ = lm.infer([kpt1, kpt2], [desc1, desc2])
+    return matches
+
+
+if __name__ == '__main__':
+    #test
+    init(128, 0.2, 20, False, True)
+    match([[1, 2], [1,3], [4,6]], [[1, 3], [1,2], [16,2]], [1, 3,6], [1,3,5], np.full((3, 128), 1), np.full((3, 128), 1), 640, 480)

corelib/src/pymatcher/rtabmap_superglue.py

@@ -0,0 +1,86 @@
+#! /usr/bin/env python3
+#
+# Drop this file in the root folder of SuperGlue git: https://github.com/magicleap/SuperGluePretrainedNetwork
+# To use with rtabmap:
+#   --Vis/CorNNType 6 --SuperGlue/Path "~/SuperGluePretrainedNetwork/rtabmap_superglue.py"
+#
+
+import random
+import numpy as np
+import torch
+
+#import sys
+#import os
+#print(os.sys.path)
+#print(sys.version)
+
+from models.matching import SuperGlue
+
+torch.set_grad_enabled(False)
+
+device = 'cpu'
+superglue = []
+
+def init(descriptorDim, matchThreshold, iterations, cuda, model):
+    print("SuperGlue python init()")
+    # Load the SuperGlue model.
+    global device
+    device = 'cuda' if torch.cuda.is_available() and cuda else 'cpu'
+    assert model == "indoor" or model == "outdoor", "Available models for SuperGlue are 'indoor' or 'outdoor'"
+    config = {
+        'superglue': {
+            'weights': model,
+            'sinkhorn_iterations': iterations,
+            'match_threshold': matchThreshold,
+            'descriptor_dim' : descriptorDim
+        }
+    }
+    global superglue
+    superglue = SuperGlue(config.get('superglue', {})).eval().to(device)
+
+
+def match(kptsFrom, kptsTo, scoresFrom, scoresTo, descriptorsFrom, descriptorsTo, imageWidth, imageHeight):
+    #print("SuperGlue python match()")
+    global device
+    kptsFrom = np.asarray(kptsFrom)
+    kptsFrom = kptsFrom[None, :, :]
+    kptsTo = np.asarray(kptsTo)
+    kptsTo = kptsTo[None, :, :]
+    scoresFrom = np.asarray(scoresFrom)
+    scoresFrom = scoresFrom[None, :]
+    scoresTo = np.asarray(scoresTo)
+    scoresTo = scoresTo[None, :]
+    descriptorsFrom = np.transpose(np.asarray(descriptorsFrom))
+    descriptorsFrom = descriptorsFrom[None, :, :]
+    descriptorsTo = np.transpose(np.asarray(descriptorsTo))
+    descriptorsTo = descriptorsTo[None, :, :]
+      
+    data = {
+       'image0': torch.rand(1, 1, imageHeight, imageWidth).to(device),
+       'image1': torch.rand(1, 1, imageHeight, imageWidth).to(device),
+       'scores0': torch.from_numpy(scoresFrom).to(device),
+       'scores1': torch.from_numpy(scoresTo).to(device),
+       'keypoints0': torch.from_numpy(kptsFrom).to(device),
+       'keypoints1': torch.from_numpy(kptsTo).to(device),
+       'descriptors0': torch.from_numpy(descriptorsFrom).to(device),
+       'descriptors1': torch.from_numpy(descriptorsTo).to(device),
+    }
+    
+
+    global superglue
+    results = superglue(data)
+
+    matches0 = results['matches0'].to('cpu').numpy()
+  
+    matchesFrom = np.nonzero(matches0!=-1)[1]
+    matchesTo = matches0[np.nonzero(matches0!=-1)]
+       
+    matchesArray = np.stack((matchesFrom, matchesTo), axis=1);
+    
+    return matchesArray
+
+
+if __name__ == '__main__':
+    #test
+    init(256, 0.2, 20, True, 'indoor')
+    match([[1, 2], [1,3]], [[1, 3], [1,2]], [1, 3], [1,3], np.full((2, 256), 1),np.full((2, 256), 1), 640, 480)

corelib/src/resources/DatabaseSchema.sql.in

@@ -87,6 +87,14 @@ CREATE TABLE Feature (
 	FOREIGN KEY (node_id) REFERENCES Node(id)
 );
 
+CREATE TABLE GlobalDescriptor (
+	node_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,
+	info BLOB,
+	data BLOB NOT NULL,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
 --
 
 CREATE TABLE Info (
@@ -162,6 +170,7 @@ END;
 -- *******************************************************************
 CREATE UNIQUE INDEX IDX_Node_id on Node (id);
 CREATE INDEX IDX_Feature_node_id on Feature (node_id);
+CREATE INDEX IDX_GlobalDescriptor_node_id on GlobalDescriptor (node_id);
 CREATE INDEX IDX_Link_from_id on Link (from_id);
 CREATE UNIQUE INDEX IDX_node_label on Node (label);
 CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);

corelib/src/superpoint_torch/SuperPoint.cc

@@ -4,6 +4,9 @@
 
 #include <superpoint_torch/SuperPoint.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
 
 
 namespace rtabmap
@@ -119,10 +122,17 @@ SPDetector::SPDetector(const std::string & modelPath, float threshold, bool nms,
 	UDEBUG("modelPath=%s thr=%f nms=%d cuda=%d", modelPath.c_str(), threshold, nms?1:0, cuda?1:0);
 	if(modelPath.empty())
 	{
+		UERROR("Model's path is empty!");
+		return;
+	}
+	std::string path = uReplaceChar(modelPath, '~', UDirectory::homeDir());
+	if(!UFile::exists(path))
+	{
+		UERROR("Model's path \"%s\" doesn't exist!", path.c_str());
 		return;
 	}
 	model_ = std::make_shared<SuperPoint>();
-	torch::load(model_, modelPath);
+	torch::load(model_, uReplaceChar(path, '~', UDirectory::homeDir()));
 
 	if(cuda && !torch::cuda::is_available())
 	{
@@ -137,8 +147,10 @@ SPDetector::~SPDetector()
 {
 }
 
-std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img)
+std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img, const cv::Mat & mask)
 {
+	UASSERT(img.type() == CV_8UC1);
+	UASSERT(mask.empty() || (mask.type() == CV_8UC1 && img.cols == mask.cols && img.rows == mask.rows));
 	detected_ = false;
 	if(model_)
 	{
@@ -158,8 +170,11 @@ std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img)
 
 		std::vector<cv::KeyPoint> keypoints_no_nms;
 		for (int i = 0; i < kpts.size(0); i++) {
-			float response = prob_[kpts[i][0]][kpts[i][1]].item<float>();
-			keypoints_no_nms.push_back(cv::KeyPoint(kpts[i][1].item<float>(), kpts[i][0].item<float>(), 8, -1, response));
+			if(mask.empty() || mask.at<unsigned char>(kpts[i][0].item<int>(), kpts[i][1].item<int>()) != 0)
+			{
+				float response = prob_[kpts[i][0]][kpts[i][1]].item<float>();
+				keypoints_no_nms.push_back(cv::KeyPoint(kpts[i][1].item<float>(), kpts[i][0].item<float>(), 8, -1, response));
+			}
 		}
 
 		detected_ = true;
@@ -203,30 +218,35 @@ cv::Mat SPDetector::compute(const std::vector<cv::KeyPoint> &keypoints)
 	{
 		cv::Mat kpt_mat(keypoints.size(), 2, CV_32F);  // [n_keypoints, 2]  (y, x)
 
+		// Based on sample_descriptors() of SuperPoint implementation in SuperGlue:
+		// https://github.com/magicleap/SuperGluePretrainedNetwork/blob/45a750e5707696da49472f1cad35b0b203325417/models/superpoint.py#L80-L92
+		float s = 8;
 		for (size_t i = 0; i < keypoints.size(); i++) {
-			kpt_mat.at<float>(i, 0) = (float)keypoints[i].pt.y;
-			kpt_mat.at<float>(i, 1) = (float)keypoints[i].pt.x;
+			kpt_mat.at<float>(i, 0) = (float)keypoints[i].pt.y - s/2 + 0.5;
+			kpt_mat.at<float>(i, 1) = (float)keypoints[i].pt.x - s/2 + 0.5;
 		}
 
 		auto fkpts = torch::from_blob(kpt_mat.data, {(long int)keypoints.size(), 2}, torch::kFloat);
 
+		float w = desc_.size(3); //W/8
+		float h = desc_.size(2); //H/8
+
 		torch::Device device(cuda_?torch::kCUDA:torch::kCPU);
 		auto grid = torch::zeros({1, 1, fkpts.size(0), 2}).to(device);  // [1, 1, n_keypoints, 2]
-		grid[0][0].slice(1, 0, 1) = 2.0 * fkpts.slice(1, 1, 2) / prob_.size(1) - 1;  // x
-		grid[0][0].slice(1, 1, 2) = 2.0 * fkpts.slice(1, 0, 1) / prob_.size(0) - 1;  // y
+		grid[0][0].slice(1, 0, 1) = 2.0 * fkpts.slice(1, 1, 2) / (w*s - s/2 - 0.5) - 1;  // x
+		grid[0][0].slice(1, 1, 2) = 2.0 * fkpts.slice(1, 0, 1) / (h*s - s/2 - 0.5) - 1;  // y
 
 		auto desc = torch::grid_sampler(desc_, grid, 0, 0, true);  // [1, 256, 1, n_keypoints]
-		desc = desc.squeeze(0).squeeze(1);  // [256, n_keypoints]
 
 		// normalize to 1
-		auto dn = torch::norm(desc, 2, 1);
-		desc = desc.div(torch::unsqueeze(dn, 1));
+		desc = torch::nn::functional::normalize(desc.reshape({1, desc_.size(1), -1})); //[1, 256, n_keypoints]
+		desc = desc.squeeze(); //[256, n_keypoints]
+		desc = desc.transpose(0, 1).contiguous(); //[n_keypoints, 256]
 
-		desc = desc.transpose(0, 1).contiguous();  // [n_keypoints, 256]
 		if(cuda_)
 			desc = desc.to(torch::kCPU);
 
-		cv::Mat desc_mat(cv::Size(desc.size(1), desc.size(0)), CV_32FC1, desc.data<float>());
+		cv::Mat desc_mat(cv::Size(desc.size(1), desc.size(0)), CV_32FC1, desc.data_ptr<float>());
 
 		return desc_mat.clone();
 	}

corelib/src/superpoint_torch/SuperPoint.h

@@ -50,7 +50,7 @@ class SPDetector {
 public:
     SPDetector(const std::string & modelPath, float threshold = 0.2f, bool nms = true, int minDistance = 4, bool cuda = false);
     virtual ~SPDetector();
-    std::vector<cv::KeyPoint> detect(const cv::Mat &img);
+    std::vector<cv::KeyPoint> detect(const cv::Mat &img, const cv::Mat & mask = cv::Mat());
     cv::Mat compute(const std::vector<cv::KeyPoint> &keypoints);
 
     void setThreshold(float threshold) {threshold_ = threshold;}

corelib/src/util3d.cpp

@@ -1271,7 +1271,11 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
 	}
 	//determine the output type
 	int fieldStates[8] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+	std::uint32_t fieldOffsets[8] = {0};
+#else
 	pcl::uint32_t fieldOffsets[8] = {0};
+#endif
 	for(unsigned int i=0; i<cloud.fields.size(); ++i)
 	{
 		if(cloud.fields[i].name.compare("x") == 0)
@@ -1436,9 +1440,15 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
 				}
 				else // XYZRGB
 				{
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+					std::uint8_t b=*(msg_data + fieldOffsets[6]);
+					std::uint8_t g=*(msg_data + fieldOffsets[6]+1);
+					std::uint8_t r=*(msg_data + fieldOffsets[6]+2);
+#else
 					pcl::uint8_t b=*(msg_data + fieldOffsets[6]);
 					pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1);
 					pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2);
+#endif
 					int * ptrInt = (int*)ptr;
 					ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
 				}
@@ -1481,9 +1491,15 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
 				}
 				else // XYZRGBNormal
 				{
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+					std::uint8_t b=*(msg_data + fieldOffsets[6]);
+					std::uint8_t g=*(msg_data + fieldOffsets[6]+1);
+					std::uint8_t r=*(msg_data + fieldOffsets[6]+2);
+#else
 					pcl::uint8_t b=*(msg_data + fieldOffsets[6]);
 					pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1);
 					pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2);
+#endif
 					int * ptrInt = (int*)ptr;
 					ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
 				}

corelib/src/util3d_correspondences.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/EpipolarGeometry.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <pcl/search/kdtree.h>
+#include <pcl/common/point_tests.h>
 
 namespace rtabmap
 {

corelib/src/util3d_features.cpp

@@ -34,12 +34,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_motion_estimation.h"
 
 #include "rtabmap/core/EpipolarGeometry.h"
+#include "opencv/five-point.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UStl.h>
 
+#include <pcl/common/point_tests.h>
+
 #include <opencv2/video/tracking.hpp>
 
 namespace rtabmap
@@ -208,14 +211,11 @@ std::map<int, cv::Point3f> generateWords3DMono(
 		const std::map<int, cv::KeyPoint> & nextWords,
 		const CameraModel & cameraModel,
 		Transform & cameraTransform,
-		int pnpIterations,
-		float pnpReprojError,
-		int pnpFlags,
-		int pnpRefineIterations,
-		float ransacParam1,
-		float ransacParam2,
+		float ransacReprojThreshold,
+		float ransacConfidence,
 		const std::map<int, cv::Point3f> & refGuess3D,
-		double * varianceOut)
+		double * varianceOut,
+		std::vector<int> * matchesOut)
 {
 	UASSERT(cameraModel.isValidForProjection());
 	std::map<int, cv::Point3f> words3D;
@@ -224,270 +224,189 @@ std::map<int, cv::Point3f> generateWords3DMono(
 	UDEBUG("pairsFound=%d/%d", pairsFound, int(refWords.size()>nextWords.size()?refWords.size():nextWords.size()));
 	if(pairsFound > 8)
 	{
-		std::vector<unsigned char> status;
-		cv::Mat F = EpipolarGeometry::findFFromWords(pairs, status, ransacParam1, ransacParam2);
-		if(!F.empty())
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
+		std::vector<cv::Point2f> refCorners(pairs.size());
+		std::vector<cv::Point2f> newCorners(pairs.size());
+		std::vector<int> indexes(pairs.size());
+		for(unsigned int i=0; i<pairs.size(); ++i)
 		{
-			//get inliers
-			//normalize coordinates
-			int oi = 0;
-			UASSERT(status.size() == pairs.size());
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
-			std::vector<cv::Point2f> refCorners(status.size());
-			std::vector<cv::Point2f> newCorners(status.size());
-			std::vector<int> indexes(status.size());
-			for(unsigned int i=0; i<status.size(); ++i)
+			if(matchesOut)
 			{
-				if(status[i])
-				{
-					refCorners[oi] = iter->second.first.pt;
-					newCorners[oi] = iter->second.second.pt;
-					indexes[oi] = iter->first;
-					++oi;
-				}
-				++iter;
+				matchesOut->push_back(iter->first);
 			}
-			refCorners.resize(oi);
-			newCorners.resize(oi);
-			indexes.resize(oi);
 
-			UDEBUG("inliers=%d/%d", oi, pairs.size());
-			if(oi > 3)
+			refCorners[i] = iter->second.first.pt;
+			newCorners[i] = iter->second.second.pt;
+			indexes[i] = iter->first;
+			++iter;
+		}
+
+		std::vector<unsigned char> status;
+		cv::Mat pts4D;
+
+		UDEBUG("Five-point algorithm");
+		/**
+		 * OpenCV five-point algorithm
+		 * David Nistér. An efficient solution to the five-point relative pose problem. Pattern Analysis and Machine Intelligence, IEEE Transactions on, 26(6):756–770, 2004.
+		 */
+		cv::Mat E = cv3::findEssentialMat(refCorners, newCorners, cameraModel.K(), cv::RANSAC, ransacConfidence, ransacReprojThreshold, status);
+
+		int essentialInliers = 0;
+		for(size_t i=0; i<status.size();++i)
+		{
+			if(status[i])
 			{
-				std::vector<cv::Point2f> refCornersRefined;
-				std::vector<cv::Point2f> newCornersRefined;
-				cv::correctMatches(F, refCorners, newCorners, refCornersRefined, newCornersRefined);
-				refCorners = refCornersRefined;
-				newCorners = newCornersRefined;
+				++essentialInliers;
+			}
+		}
+		Transform cameraTransformGuess = cameraTransform;
+		if(!E.empty())
+		{
+			UDEBUG("essential inliers=%d/%d", essentialInliers, (int)status.size());
+			cv::Mat R,t;
+			cv3::recoverPose(E, refCorners, newCorners, cameraModel.K(), R, t, 50, status, pts4D);
+			if(!R.empty() && !t.empty())
+			{
+				cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
+				R.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
+				P.at<double>(0,3) = t.at<double>(0);
+				P.at<double>(1,3) = t.at<double>(1);
+				P.at<double>(2,3) = t.at<double>(2);
 
-				cv::Mat x(3, (int)refCorners.size(), CV_64FC1);
-				cv::Mat xp(3, (int)refCorners.size(), CV_64FC1);
-				for(unsigned int i=0; i<refCorners.size(); ++i)
+				cameraTransform = Transform(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0),
+						R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1),
+						R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2));
+				UDEBUG("t (cam frame)=%s", cameraTransform.prettyPrint().c_str());
+				UDEBUG("base->cam=%s", cameraModel.localTransform().prettyPrint().c_str());
+				cameraTransform = cameraModel.localTransform() * cameraTransform.inverse() * cameraModel.localTransform().inverse();
+				UDEBUG("t (base frame)=%s", cameraTransform.prettyPrint().c_str());
+
+				UASSERT((int)indexes.size() == pts4D.cols && pts4D.rows == 4 && status.size() == indexes.size());
+				for(unsigned int i=0; i<indexes.size(); ++i)
 				{
-					x.at<double>(0, i) = refCorners[i].x;
-					x.at<double>(1, i) = refCorners[i].y;
-					x.at<double>(2, i) = 1;
-
-					xp.at<double>(0, i) = newCorners[i].x;
-					xp.at<double>(1, i) = newCorners[i].y;
-					xp.at<double>(2, i) = 1;
-				}
-
-				cv::Mat K = cameraModel.K();
-				cv::Mat Kinv = K.inv();
-				cv::Mat E = K.t()*F*K;
-				cv::Mat x_norm = Kinv * x;
-				cv::Mat xp_norm = Kinv * xp;
-				x_norm = x_norm.rowRange(0,2);
-				xp_norm = xp_norm.rowRange(0,2);
-
-				cv::Mat P = EpipolarGeometry::findPFromE(E, x_norm, xp_norm);
-				if(!P.empty())
-				{
-					cv::Mat P0 = cv::Mat::zeros(3, 4, CV_64FC1);
-					P0.at<double>(0,0) = 1;
-					P0.at<double>(1,1) = 1;
-					P0.at<double>(2,2) = 1;
-
-					bool useCameraTransformGuess = !cameraTransform.isNull();
-					//if camera transform is set, use it instead of the computed one from epipolar geometry
-					if(useCameraTransformGuess)
+					if(status[i])
 					{
-						Transform t = (cameraModel.localTransform().inverse()*cameraTransform*cameraModel.localTransform()).inverse();
-
-						if(ULogger::level() == ULogger::kDebug)
-						{
-							UDEBUG("Guess = %s", t.prettyPrint().c_str());
-							UDEBUG("Epipolar = %s", Transform(P).prettyPrint().c_str());
-							Transform PT = Transform(P);
-							float scale = t.getNorm()/PT.getNorm();
-							UDEBUG("Scale= %f", scale);
-							PT.x()*=scale;
-							PT.y()*=scale;
-							PT.z()*=scale;
-							UDEBUG("Epipolar scaled= %s", PT.prettyPrint().c_str());
-						}
-
-						P = (cv::Mat_<double>(3,4) <<
-								(double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.x(),
-								(double)t.r21(), (double)t.r22(), (double)t.r23(), (double)t.y(),
-								(double)t.r31(), (double)t.r32(), (double)t.r33(), (double)t.z());
-					}
-
-					// triangulate the points
-					//std::vector<double> reprojErrors;
-					//std::vector<cv::Point3f> cloud;
-					//EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
-					cv::Mat pts4D;
-					cv::triangulatePoints(P0, P, x_norm, xp_norm, pts4D);
-
-					UASSERT((int)indexes.size() == pts4D.cols && pts4D.rows == 4);
-					for(unsigned int i=0; i<indexes.size(); ++i)
-					{
-						//if(cloud->at(i).z > 0)
-						//{
-						//	words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(cloud->at(i), localTransform)));
-						//}
 						pts4D.col(i) /= pts4D.at<double>(3,i);
 						if(pts4D.at<double>(2,i) > 0)
 						{
 							words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(cv::Point3f(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), cameraModel.localTransform())));
 						}
 					}
-
-					UDEBUG("ref guess=%d", (int)refGuess3D.size());
-					if(refGuess3D.size())
-					{
-						// scale estimation
-						std::vector<cv::Point3f> inliersRef;
-						std::vector<cv::Point3f> inliersRefGuess;
-						util3d::findCorrespondences(
-								words3D,
-								refGuess3D,
-								inliersRef,
-								inliersRefGuess,
-								0);
-
-						if(inliersRef.size())
-						{
-							// estimate the scale
-							float scale = 1.0f;
-							float variance = 1.0f;
-							if(!useCameraTransformGuess)
-							{
-								std::multimap<float, float> scales; // <variance, scale>
-								for(unsigned int i=0; i<inliersRef.size(); ++i)
-								{
-									// using x as depth, assuming we are in global referential
-									float s = inliersRefGuess.at(i).x/inliersRef.at(i).x;
-									std::vector<float> errorSqrdDists(inliersRef.size());
-									for(unsigned int j=0; j<inliersRef.size(); ++j)
-									{
-										cv::Point3f refPt = inliersRef.at(j);
-										refPt.x *= s;
-										refPt.y *= s;
-										refPt.z *= s;
-										const cv::Point3f & newPt = inliersRefGuess.at(j);
-										errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
-									}
-									std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-									double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
-									float var = 2.1981 * median_error_sqr;
-									//UDEBUG("scale %d = %f variance = %f", (int)i, s, variance);
-
-									scales.insert(std::make_pair(var, s));
-								}
-								scale = scales.begin()->second;
-								variance = scales.begin()->first;;
-							}
-							else
-							{
-								//compute variance at scale=1
-								std::vector<float> errorSqrdDists(inliersRef.size());
-								for(unsigned int j=0; j<inliersRef.size(); ++j)
-								{
-									const cv::Point3f & refPt = inliersRef.at(j);
-									const cv::Point3f & newPt = inliersRefGuess.at(j);
-									errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
-								}
-								std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-								double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
-								 variance = 2.1981 * median_error_sqr;
-							}
-
-							UDEBUG("scale used = %f (variance=%f)", scale, variance);
-							if(varianceOut)
-							{
-								*varianceOut = variance;
-							}
-
-							if(!useCameraTransformGuess)
-							{
-								std::vector<cv::Point3f> objectPoints(indexes.size());
-								std::vector<cv::Point2f> imagePoints(indexes.size());
-								int oi2=0;
-								UASSERT(indexes.size() == newCorners.size());
-								for(unsigned int i=0; i<indexes.size(); ++i)
-								{
-									std::map<int, cv::Point3f>::iterator iter = words3D.find(indexes[i]);
-									if(iter!=words3D.end() && util3d::isFinite(iter->second))
-									{
-										iter->second.x *= scale;
-										iter->second.y *= scale;
-										iter->second.z *= scale;
-										objectPoints[oi2].x = iter->second.x;
-										objectPoints[oi2].y = iter->second.y;
-										objectPoints[oi2].z = iter->second.z;
-										imagePoints[oi2] = newCorners[i];
-										++oi2;
-									}
-								}
-								objectPoints.resize(oi2);
-								imagePoints.resize(oi2);
-
-								//PnPRansac
-								Transform guess = cameraModel.localTransform().inverse();
-								cv::Mat R = (cv::Mat_<double>(3,3) <<
-										(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
-										(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
-										(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
-								cv::Mat rvec(1,3, CV_64FC1);
-								cv::Rodrigues(R, rvec);
-								cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
-								std::vector<int> inliersV;
-								util3d::solvePnPRansac(
-										objectPoints,
-										imagePoints,
-										K,
-										cv::Mat(),
-										rvec,
-										tvec,
-										true,
-										pnpIterations,
-										pnpReprojError,
-										0, // min inliers
-										inliersV,
-										pnpFlags,
-										pnpRefineIterations);
-
-								UDEBUG("PnP inliers = %d / %d", (int)inliersV.size(), (int)objectPoints.size());
-
-								if(inliersV.size())
-								{
-									cv::Rodrigues(rvec, R);
-									Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
-												   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
-												   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
-
-									cameraTransform = (cameraModel.localTransform() * pnp).inverse();
-								}
-								else
-								{
-									UWARN("No inliers after PnP!");
-								}
-							}
-						}
-						else
-						{
-							UWARN("Cannot compute the scale, no points corresponding between the generated ref words and words guess");
-						}
-					}
-					else if(!useCameraTransformGuess)
-					{
-						cv::Mat R, T;
-						EpipolarGeometry::findRTFromP(P, R, T);
-
-						Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0),
-									R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
-									R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
-
-						cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform();
-					}
 				}
 			}
 		}
+		else
+		{
+			UDEBUG("Failed to find essential matrix");
+		}
+
+		if(!cameraTransform.isNull())
+		{
+			UDEBUG("words3D=%d refGuess3D=%d cameraGuess=%s", (int)words3D.size(), (int)refGuess3D.size(), cameraTransformGuess.prettyPrint().c_str());
+
+			// estimate the scale and variance
+			float scale = 1.0f;
+			if(!cameraTransformGuess.isNull())
+			{
+				scale = cameraTransformGuess.getNorm()/cameraTransform.getNorm();
+			}
+			float variance = 1.0f;
+
+			std::vector<cv::Point3f> inliersRef;
+			std::vector<cv::Point3f> inliersRefGuess;
+			if(!refGuess3D.empty())
+			{
+				util3d::findCorrespondences(
+						words3D,
+						refGuess3D,
+						inliersRef,
+						inliersRefGuess,
+						0);
+			}
+
+			if(!inliersRef.empty())
+			{
+				UDEBUG("inliersRef=%d", (int)inliersRef.size());
+				if(cameraTransformGuess.isNull())
+				{
+					std::multimap<float, float> scales; // <variance, scale>
+					for(unsigned int i=0; i<inliersRef.size(); ++i)
+					{
+						// using x as depth, assuming we are in global referential
+						float s = inliersRefGuess.at(i).x/inliersRef.at(i).x;
+						std::vector<float> errorSqrdDists(inliersRef.size());
+						for(unsigned int j=0; j<inliersRef.size(); ++j)
+						{
+							cv::Point3f refPt = inliersRef.at(j);
+							refPt.x *= s;
+							refPt.y *= s;
+							refPt.z *= s;
+							const cv::Point3f & newPt = inliersRefGuess.at(j);
+							errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
+						}
+						std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
+						double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 2];
+						float var = 2.1981 * median_error_sqr;
+						//UDEBUG("scale %d = %f variance = %f", (int)i, s, variance);
+
+						scales.insert(std::make_pair(var, s));
+					}
+					scale = scales.begin()->second;
+					variance = scales.begin()->first;
+				}
+				else if(!cameraTransformGuess.isNull())
+				{
+					// use scale from guess
+					//compute variance
+					std::vector<float> errorSqrdDists(inliersRef.size());
+					for(unsigned int j=0; j<inliersRef.size(); ++j)
+					{
+						cv::Point3f refPt = inliersRef.at(j);
+						refPt.x *= scale;
+						refPt.y *= scale;
+						refPt.z *= scale;
+						const cv::Point3f & newPt = inliersRefGuess.at(j);
+						errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
+					}
+					std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
+					double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 2];
+					variance = 2.1981 * median_error_sqr;
+				}
+			}
+			else if(!refGuess3D.empty())
+			{
+				UWARN("Cannot compute variance, no points corresponding between "
+						"the generated ref words (%d) and words guess (%d)",
+						(int)words3D.size(), (int)refGuess3D.size());
+			}
+
+			if(scale!=1.0f)
+			{
+				// Adjust output transform and points based on scale found
+				cameraTransform.x()*=scale;
+				cameraTransform.y()*=scale;
+				cameraTransform.z()*=scale;
+
+				UASSERT(indexes.size() == newCorners.size());
+				for(unsigned int i=0; i<indexes.size(); ++i)
+				{
+					std::map<int, cv::Point3f>::iterator iter = words3D.find(indexes[i]);
+					if(iter!=words3D.end() && util3d::isFinite(iter->second))
+					{
+						iter->second.x *= scale;
+						iter->second.y *= scale;
+						iter->second.z *= scale;
+					}
+				}
+			}
+			UDEBUG("scale used = %f (variance=%f)", scale, variance);
+			if(varianceOut)
+			{
+				*varianceOut = variance;
+			}
+		}
 	}
-	UDEBUG("wordsSet=%d / %d", (int)words3D.size(), (int)refWords.size());
+	UDEBUG("wordsSet=%d / %d", (int)words3D.size(), pairsFound);
 
 	return words3D;
 }

corelib/src/util3d_motion_estimation.cpp

@@ -94,8 +94,9 @@ Transform estimateMotion3DTo2D(
 	imagePoints.resize(oi);
 	matches.resize(oi);
 
-	UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s",
-			(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(), guess.prettyPrint().c_str());
+	UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d",
+			(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(),
+			guess.prettyPrint().c_str(), reprojError, iterations);
 
 	if((int)matches.size() >= minInliers)
 	{

corelib/src/util3d_surface.cpp

@@ -1007,7 +1007,11 @@ pcl::TextureMesh::Ptr concatenateTextureMeshes(const std::list<pcl::TextureMesh:
 			// append point cloud
 			int polygonStep = output->cloud.height * output->cloud.width;
 			pcl::PCLPointCloud2 tmp;
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+			pcl::concatenate(output->cloud, iter->get()->cloud, tmp);
+#else
 			pcl::concatenatePointCloud(output->cloud, iter->get()->cloud, tmp);
+#endif
 			output->cloud = tmp;
 
 			UASSERT((*iter)->tex_polygons.size() == (*iter)->tex_coordinates.size() &&
@@ -1553,7 +1557,7 @@ cv::Mat mergeTextures(
 					}
 					else if(memory)
 					{
-						SensorData data = memory->getSignatureDataConst(textureId, true, false, false, false);
+						SensorData data = memory->getNodeData(textureId, true, false, false, false);
 						std::vector<CameraModel> models = data.cameraModels();
 						StereoCameraModel stereoModel = data.stereoCameraModel();
 						if(models.size()>=1 &&
@@ -1682,7 +1686,7 @@ cv::Mat mergeTextures(
 								}
 								else if(memory)
 								{
-									SensorData data = memory->getSignatureDataConst(textures[t].first, true, false, false, false);
+									SensorData data = memory->getNodeData(textures[t].first, true, false, false, false);
 									models = data.cameraModels();
 									data.uncompressDataConst(&image, 0);
 								}
@@ -2291,7 +2295,7 @@ bool multiBandTexturing(
 		}
 		else if(memory)
 		{
-			SensorData data = memory->getSignatureDataConst(camId, true, false, false, false);
+			SensorData data = memory->getNodeData(camId, true, false, false, false);
 			models = data.cameraModels();
 			if(models.empty() && data.stereoCameraModel().isValidForProjection())
 			{
@@ -2993,7 +2997,7 @@ float computeNormalsComplexity(
 	int oi = 0;
 	bool doTransform = false;
 	Transform tn;
-	if(!t.isIdentity())
+	if(!t.isIdentity() && !t.isNull())
 	{
 		tn = t.rotation();
 		doTransform = true;
@@ -3202,12 +3206,18 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
 	mls.setComputeNormals (true);
 	if(polygonialOrder > 0)
 	{
+#if PCL_VERSION_COMPARE(<, 1, 10, 0)
 		mls.setPolynomialFit (true);
+#endif
 		mls.setPolynomialOrder(polygonialOrder);
 	}
 	else
 	{
+#if PCL_VERSION_COMPARE(<, 1, 10, 0)
 		mls.setPolynomialFit (false);
+#else
+		mls.setPolynomialOrder(1);
+#endif
 	}
 	UASSERT(upsamplingMethod >= mls.NONE &&
 			upsamplingMethod <= mls.VOXEL_GRID_DILATION);

examples/RGBDMapping/main.cpp

@@ -216,7 +216,7 @@ int main(int argc, char * argv[])
 	std::map<int, Signature> nodes;
 	std::map<int, Transform> optimizedPoses;
 	std::multimap<int, Link> links;
-	rtabmap->get3DMap(nodes, optimizedPoses, links, true, true);
+	rtabmap->getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true);
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 	for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
 	{

guilib/include/rtabmap/gui/CloudViewer.h

@@ -88,7 +88,8 @@ public:
 			bool rgb,
 			bool hasNormals,
 			bool hasIntensity,
-			const QColor & color = QColor());
+			const QColor & color = QColor(),
+			int viewport = 1);
 
 	bool addCloud(
 			const std::string & id,
@@ -256,10 +257,12 @@ public:
 
 	void addOrUpdateFrustum(
 			const std::string & id,
-			const Transform & transform,
+			const Transform & pose,
 			const Transform & localTransform,
 			double scale,
-			const QColor & color = QColor());
+			const QColor & color = QColor(),
+			float fovX=1.1,
+			float fovY=0.85);
 	bool updateFrustumPose(
 			const std::string & id,
 			const Transform & pose);
@@ -317,6 +320,7 @@ public:
 	void setBackfaceCulling(bool enabled, bool frontfaceCulling);
 	void setPolygonPicking(bool enabled);
 	void setRenderingRate(double rate);
+	void setEDLShading(bool on);
 	void setLighting(bool on);
 	void setShading(bool on);
 	void setEdgeVisibility(bool visible);
@@ -325,6 +329,7 @@ public:
 	bool isBackfaceCulling() const;
 	bool isFrontfaceCulling() const;
 	bool isPolygonPicking() const;
+	bool isEDLShadingOn() const;
 	bool isLightingOn() const;
 	bool isShadingOn() const;
 	bool isEdgeVisible() const;
@@ -420,6 +425,7 @@ private:
 	QAction * _aSetIntensityMaximum;
     QAction * _aSetBackgroundColor;
     QAction * _aSetRenderingRate;
+    QAction * _aSetEDLShading;
     QAction * _aSetLighting;
     QAction * _aSetFlatShading;
     QAction * _aSetEdgeVisibility;

guilib/include/rtabmap/gui/MainWindow.h

@@ -172,6 +172,7 @@ protected Q_SLOTS:
 	void selectOpenni2();
 	void selectFreenect2();
 	void selectK4W2();
+	void selectK4A();
 	void selectRealSense();
 	void selectRealSense2();
 	void selectRealSense2Stereo();
@@ -329,6 +330,7 @@ private:
 	QString _newDatabasePath;
 	QString _newDatabasePathOutput;
 	QString _openedDatabasePath;
+	QString _defaultOpenDatabasePath;
 	bool _databaseUpdated;
 	bool _odomImageShow;
 	bool _odomImageDepthShow;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -161,6 +161,7 @@ public:
 	bool isWordsCountGraphView() const;
 	bool isLocalizationsCountGraphView() const;
 	int getOdomRegistrationApproach() const;
+	double getOdomF2MGravitySigma() const;
 	bool isOdomDisabled() const;
 	bool isGroundTruthAligned() const;
 
@@ -329,6 +330,7 @@ private Q_SLOTS:
 	void updatePredictionPlot();
 	void updateKpROI();
 	void updateStereoDisparityVisibility();
+	void updateFeatureMatchingVisibility();
 	void useOdomFeatures();
 	void changeWorkingDirectory();
 	void changeDictionaryPath();
@@ -336,7 +338,9 @@ private Q_SLOTS:
 	void changeOdometryOKVISConfigPath();
 	void changeOdometryVINSConfigPath();
 	void changeIcpPMConfigPath();
-	void changeSPTorchModelPath();
+	void changeSuperPointModelPath();
+	void changePyMatcherPath();
+	void changePyMatcherModel();
 	void readSettingsEnd();
 	void setupTreeView();
 	void updateBasicParameter();

guilib/include/rtabmap/utilite/UPlot.h

@@ -37,6 +37,7 @@ class QGraphicsView;
 class QGraphicsScene;
 class QGraphicsItem;
 class QFormLayout;
+class QScrollArea;
 
 /**
  * UPlotItem is a QGraphicsEllipseItem and can be inherited to do custom behaviors
@@ -353,6 +354,7 @@ public:
 	virtual ~UPlotLegendItem();
 	const UPlotCurve * curve() const {return _curve;}
 	QPixmap createSymbol(const QPen & pen, const QBrush & brush);
+	void showStdDevMeanMax(bool shown);
 
 Q_SIGNALS:
 	void legendItemRemoved(const UPlotCurve *);
@@ -418,6 +420,9 @@ private:
 	QMenu * _menu;
 	QAction * _aUseFlatButtons;
 	QAction * _aCopyAllCurvesToClipboard;
+	QAction * _aShowAllStdDevMeanMax;
+	QLayout * _contentLayout;
+	QScrollArea * _scrollArea;
 };
 
 

guilib/src/AboutDialog.cpp

@@ -47,12 +47,25 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	version.append(" [DEMO]");
 #endif
 	QString cv_version = CV_VERSION;
-#ifdef RTABMAP_NONFREE
+#if CV_MAJOR_VERSION < 3
+  #ifdef RTABMAP_NONFREE
 	cv_version.append(" [With nonfree]");
 	_ui->label_opencv_license->setText("Not Commercial");
-#else
+  #else
 	cv_version.append(" [Without nonfree]");
 	_ui->label_opencv_license->setText("BSD");
+  #endif
+#elif defined(HAVE_OPENCV_XFEATURES2D)
+  #ifdef RTABMAP_NONFREE
+	cv_version.append(" [With xfeatures2d, nonfree]");
+	_ui->label_opencv_license->setText("Not Commercial");
+  #else
+	cv_version.append(" [With xfeatures2d]");
+	_ui->label_opencv_license->setText("BSD");
+  #endif
+#else
+	cv_version.append(" [Without xfeatures2d and nonfree]");
+	_ui->label_opencv_license->setText("BSD");
 #endif
 	_ui->label_version->setText(version);
 	_ui->label_opencv_version->setText(cv_version);
@@ -66,13 +79,20 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_orboctree->setText("No");
 	_ui->label_orboctree_license->setEnabled(false);
 #endif
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	_ui->label_sptorch->setText("Yes");
 	_ui->label_sptorch_license->setEnabled(true);
 #else
 	_ui->label_sptorch->setText("No");
 	_ui->label_sptorch_license->setEnabled(false);
 #endif
+#ifdef RTABMAP_PYMATCHER
+	_ui->label_pymatcher->setText("Yes");
+	_ui->label_pymatcher_license->setEnabled(true);
+#else
+	_ui->label_pymatcher->setText("No");
+	_ui->label_pymatcher_license->setEnabled(false);
+#endif
 #ifdef RTABMAP_FASTCV
 	_ui->label_fastcv->setText("Yes");
 	_ui->label_fastcv_license->setEnabled(true);

guilib/src/CloudViewer.cpp

@@ -74,6 +74,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <vtkQuad.h>
 #include <opencv/vtkImageMatSource.h>
 
+#if VTK_MAJOR_VERSION >= 7
+#include <vtkEDLShading.h>
+#include <vtkRenderStepsPass.h>
+#include <vtkOpenGLRenderer.h>
+#endif
+
 #ifdef RTABMAP_OCTOMAP
 #include <rtabmap/core/OctoMap.h>
 #endif
@@ -102,6 +108,7 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 		_aSetNormalsScale(0),
 		_aSetBackgroundColor(0),
 		_aSetRenderingRate(0),
+		_aSetEDLShading(0),
 		_aSetLighting(0),
 		_aSetFlatShading(0),
 		_aSetEdgeVisibility(0),
@@ -131,6 +138,7 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 	int argc = 0;
 	UASSERT(style!=0);
 	style->setCloudViewer(this);
+	style->SetAutoAdjustCameraClippingRange(true);
 	_visualizer = new pcl::visualization::PCLVisualizer(
 		argc, 
 		0, 
@@ -141,8 +149,10 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 	_visualizer->setShowFPS(false);
 	
 	int viewport;
-	_visualizer->createViewPort (0,0,1.0, 1.0, viewport); // all 3d objects here
-	_visualizer->createViewPort (0,0,1.0, 1.0, viewport); // text overlay
+	// Layer 0: unavailable layer, used as "all" by PCLVisualizer
+	_visualizer->createViewPort (0,0,1.0, 1.0, viewport); // Layer 1: all clouds here
+	_visualizer->createViewPort (0,0,1.0, 1.0, viewport); // Layer 2: all 3d objects here
+	_visualizer->createViewPort (0,0,1.0, 1.0, viewport); // Layer 3: text overlay
 	_visualizer->getRendererCollection()->InitTraversal ();
 	vtkRenderer* renderer = NULL;
 	int i =0;
@@ -151,11 +161,22 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 		renderer->SetLayer(i);
 		if(i==1)
 		{
+#if VTK_MAJOR_VERSION >= 7
+			renderer->PreserveColorBufferOff();
+#endif
+			renderer->PreserveDepthBufferOff();
 			_visualizer->getInteractorStyle()->SetDefaultRenderer(renderer);
 		}
+		else if(i==2)
+		{
+#if VTK_MAJOR_VERSION >= 7
+			renderer->PreserveColorBufferOn();
+#endif
+			renderer->PreserveDepthBufferOn();
+		}
 		++i;
 	}
-	_visualizer->getRenderWindow()->SetNumberOfLayers(3);
+	_visualizer->getRenderWindow()->SetNumberOfLayers(4);
 
 	this->SetRenderWindow(_visualizer->getRenderWindow());
 
@@ -262,9 +283,17 @@ void CloudViewer::createMenu()
 	_aSetIntensityMaximum = new QAction("Set maximum absolute intensity...", this);
 	_aSetBackgroundColor = new QAction("Set background color...", this);	
 	_aSetRenderingRate = new QAction("Set rendering rate...", this);
+	_aSetEDLShading = new QAction("Eye-Dome Lighting Shading", this);
+	_aSetEDLShading->setCheckable(true);
+	_aSetEDLShading->setChecked(false);
+
 	_aSetLighting = new QAction("Lighting", this);
 	_aSetLighting->setCheckable(true);
 	_aSetLighting->setChecked(false);
+#if VTK_MAJOR_VERSION < 7
+	_aSetLighting->setEnabled(false);
+#endif
+
 	_aSetFlatShading = new QAction("Flat Shading", this);
 	_aSetFlatShading->setCheckable(true);
 	_aSetFlatShading->setChecked(false);
@@ -326,6 +355,7 @@ void CloudViewer::createMenu()
 	_menu->addMenu(scanMenu);
 	_menu->addAction(_aSetBackgroundColor);
 	_menu->addAction(_aSetRenderingRate);
+	_menu->addAction(_aSetEDLShading);
 	_menu->addAction(_aSetLighting);
 	_menu->addAction(_aSetFlatShading);
 	_menu->addAction(_aSetEdgeVisibility);
@@ -619,7 +649,8 @@ bool CloudViewer::addCloud(
 		bool rgb,
 		bool hasNormals,
 		bool hasIntensity,
-		const QColor & color)
+		const QColor & color,
+		int viewport)
 {
 	int previousColorIndex = -1;
 	if(_addedClouds.contains(id))
@@ -636,7 +667,7 @@ bool CloudViewer::addCloud(
 		pcl::PointCloud<pcl::PointNormal>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointNormal>);
 		pcl::fromPCLPointCloud2 (*binaryCloud, *cloud_xyz);
 		std::string idNormals = id + "-normals";
-		if(_visualizer->addPointCloudNormals<pcl::PointNormal>(cloud_xyz, _normalsStep, _normalsScale, idNormals, 0))
+		if(_visualizer->addPointCloudNormals<pcl::PointNormal>(cloud_xyz, _normalsStep, _normalsScale, idNormals, viewport))
 		{
 			_visualizer->updatePointCloudPose(idNormals, pose.toEigen3f());
 			_addedClouds.insert(idNormals, pose);
@@ -646,7 +677,7 @@ bool CloudViewer::addCloud(
 	// add random color channel
 	 pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::Ptr colorHandler;
 	 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (binaryCloud));
-	 if(_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1))
+	 if(_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport))
 	 {
 		QColor c = Qt::gray;
 		if(color.isValid())
@@ -654,27 +685,27 @@ bool CloudViewer::addCloud(
 			c = color;
 		}
 		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, c.red(), c.green(), c.blue()));
-		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
+		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 
 		// x,y,z
 		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "x"));
-		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
+		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "y"));
-		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
+		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "z"));
-		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
+		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 
 		if(rgb)
 		{
 			//rgb
 			colorHandler.reset(new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2>(binaryCloud));
-			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 		}
 		else if(hasIntensity)
 		{
 			//intensity
 			colorHandler.reset(new PointCloudColorHandlerIntensityField(binaryCloud, _intensityAbsMax, _aSetIntensityRedColormap->isChecked()));
-			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 		}
 		else if(previousColorIndex == 5)
 		{
@@ -685,11 +716,11 @@ bool CloudViewer::addCloud(
 		{
 			//normals
 			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_x"));
-			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_y"));
-			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_z"));
-			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 		}
 		else if(previousColorIndex > 5)
 		{
@@ -1028,11 +1059,11 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bo
 
 			vtkSmartPointer<vtkGlyph3DMapper> mapper = vtkSmartPointer<vtkGlyph3DMapper>::New();
 			mapper->SetSourceConnection(cubeSource->GetOutputPort());
-	#if VTK_MAJOR_VERSION <= 5
+#if VTK_MAJOR_VERSION <= 5
 			mapper->SetInputConnection(polydata->GetProducerPort());
-	#else
+#else
 			mapper->SetInputData(polydata);
-	#endif
+#endif
 			mapper->SetScalarRange(0, obstacles->size() - 1);
 			mapper->SetLookupTable(lut);
 			mapper->ScalingOff();
@@ -1444,7 +1475,7 @@ void CloudViewer::addOrUpdateCoordinate(
 	{
 		_coordinates.insert(id);
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
-		_visualizer->addCoordinateSystem(scale, transform.toEigen3f(), id, foreground?2:1);
+		_visualizer->addCoordinateSystem(scale, transform.toEigen3f(), id, foreground?3:2);
 #else
 		// Well, on older versions, just update the main coordinate
 		_visualizer->addCoordinateSystem(scale, transform.toEigen3f(), 0);
@@ -1532,11 +1563,11 @@ void CloudViewer::addOrUpdateLine(
 
 		if(arrow)
 		{
-			_visualizer->addArrow(pt2, pt1, c.redF(), c.greenF(), c.blueF(), false, id, foreground?2:1);
+			_visualizer->addArrow(pt2, pt1, c.redF(), c.greenF(), c.blueF(), false, id, foreground?3:2);
 		}
 		else
 		{
-			_visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id, foreground?2:1);
+			_visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id, foreground?3:2);
 		}
 		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
 	}
@@ -1593,7 +1624,7 @@ void CloudViewer::addOrUpdateSphere(
 		}
 
 		pcl::PointXYZ center(pose.x(), pose.y(), pose.z());
-		_visualizer->addSphere(center, radius, c.redF(), c.greenF(), c.blueF(), id, foreground?2:1);
+		_visualizer->addSphere(center, radius, c.redF(), c.greenF(), c.blueF(), id, foreground?3:2);
 		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
 	}
 }
@@ -1650,7 +1681,7 @@ void CloudViewer::addOrUpdateCube(
 		{
 			c = color;
 		}
-		_visualizer->addCube(Eigen::Vector3f(pose.x(), pose.y(), pose.z()), pose.getQuaternionf(), width, height, depth, id, foreground?2:1);
+		_visualizer->addCube(Eigen::Vector3f(pose.x(), pose.y(), pose.z()), pose.getQuaternionf(), width, height, depth, id, foreground?3:2);
 		if(wireframe)
 		{
 			_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, id);
@@ -1780,7 +1811,7 @@ void CloudViewer::addOrUpdateQuad(
 		int i = 0;
 		while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
 		{
-			if ((foreground?2:1) == i)               // add the actor only to the specified viewport
+			if ((foreground?3:2) == i)               // add the actor only to the specified viewport
 			{
 				renderer->AddActor (actor);
 			}
@@ -1831,18 +1862,20 @@ static const float frustum_vertices[] = {
     0.0f,  0.0f, 0.0f,
 	1.0f, 1.0f, 1.0f,
 	1.0f, -1.0f, 1.0f,
-	1.0f, -1.0f, -1.0f,
-	1.0f, 1.0f, -1.0f};
+	-1.0f, -1.0f, 1.0f,
+	-1.0f, 1.0f, 1.0f};
 
 static const int frustum_indices[] = {
     1, 2, 3, 4, 1, 0, 2, 0, 3, 0, 4};
 
 void CloudViewer::addOrUpdateFrustum(
 			const std::string & id,
-			const Transform & transform,
+			const Transform & pose,
 			const Transform & localTransform,
 			double scale,
-			const QColor & color)
+			const QColor & color,
+			float fovX,
+			float fovY)
 {
 	if(id.empty())
 	{
@@ -1854,7 +1887,7 @@ void CloudViewer::addOrUpdateFrustum(
 	this->removeFrustum(id);
 #endif
 
-	if(!transform.isNull())
+	if(!pose.isNull())
 	{
 		if(_frustums.find(id)==_frustums.end())
 		{
@@ -1865,9 +1898,9 @@ void CloudViewer::addOrUpdateFrustum(
 			frustumSize/=3;
 			pcl::PointCloud<pcl::PointXYZ> frustumPoints;
 			frustumPoints.resize(frustumSize);
-			float scaleX = 0.5f * scale;
-			float scaleY = 0.4f * scale; //4x3 arbitrary ratio
-			float scaleZ = 0.3f * scale;
+			float scaleX = tan((fovX>0?fovX:1.1)/2.0f) * scale;
+			float scaleY = tan((fovY>0?fovY:0.85)/2.0f) * scale;
+			float scaleZ = scale;
 			QColor c = Qt::gray;
 			if(color.isValid())
 			{
@@ -1876,9 +1909,9 @@ void CloudViewer::addOrUpdateFrustum(
 			Transform opticalRotInv(0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0);
 
 #if PCL_VERSION_COMPARE(<, 1, 7, 2)
-			Eigen::Affine3f t = (transform*localTransform*opticalRotInv).toEigen3f();
+			Eigen::Affine3f t = (pose*localTransform).toEigen3f();
 #else
-			Eigen::Affine3f t = (localTransform*opticalRotInv).toEigen3f();
+			Eigen::Affine3f t = (localTransform).toEigen3f();
 #endif
 			for(int i=0; i<frustumSize; ++i)
 			{
@@ -1897,12 +1930,12 @@ void CloudViewer::addOrUpdateFrustum(
 			}
 			pcl::toPCLPointCloud2(frustumPoints, mesh.cloud);
 			mesh.polygons.push_back(vertices);
-			_visualizer->addPolylineFromPolygonMesh(mesh, id, 1);
+			_visualizer->addPolylineFromPolygonMesh(mesh, id, 2);
 			_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
 			_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
 		}
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
-		if(!this->updateFrustumPose(id, transform))
+		if(!this->updateFrustumPose(id, pose))
 		{
 			UERROR("Failed updating pose of frustum %s!?", id.c_str());
 		}
@@ -2011,11 +2044,13 @@ void CloudViewer::addOrUpdateGraph(
 		}
 		pcl::toPCLPointCloud2(*graph, mesh.cloud);
 		mesh.polygons.push_back(vertices);
-		_visualizer->addPolylineFromPolygonMesh(mesh, id, 1);
+		_visualizer->addPolylineFromPolygonMesh(mesh, id, 2);
 		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.redF(), color.greenF(), color.blueF(), id);
 		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, color.alphaF(), id);
 
-		this->addCloud(id+"_nodes", graph, Transform::getIdentity(), color);
+		pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
+		pcl::toPCLPointCloud2(*graph, *binaryCloud);
+		this->addCloud(id+"_nodes", binaryCloud, Transform::getIdentity(), false, false, false, color, 2);
 		this->setCloudPointSize(id+"_nodes", 5);
 	}
 }
@@ -2073,7 +2108,7 @@ void CloudViewer::addOrUpdateText(
 				color.greenF(),
 				color.blueF(),
 				id,
-				foreground?2:1);
+				foreground?3:2);
 	}
 }
 
@@ -2441,6 +2476,41 @@ void CloudViewer::setRenderingRate(double rate)
 	_visualizer->getInteractorStyle()->GetInteractor()->SetDesiredUpdateRate(_renderingRate);
 }
 
+void CloudViewer::setEDLShading(bool on)
+{
+#if VTK_MAJOR_VERSION >= 7
+	_aSetEDLShading->setChecked(on);
+	_visualizer->getRendererCollection()->InitTraversal ();
+	vtkRenderer* renderer = NULL;
+	renderer = _visualizer->getRendererCollection()->GetNextItem ();
+	renderer = _visualizer->getRendererCollection()->GetNextItem (); // Get Layer 1
+	UASSERT(renderer);
+
+	vtkOpenGLRenderer* glrenderer = vtkOpenGLRenderer::SafeDownCast(renderer);
+	UASSERT(glrenderer);
+	if(on)
+	{
+		// EDL shader
+		vtkSmartPointer<vtkRenderStepsPass> basicPasses = vtkSmartPointer<vtkRenderStepsPass>::New ();
+		vtkSmartPointer<vtkEDLShading> edl = vtkSmartPointer<vtkEDLShading>::New ();
+		edl->SetDelegatePass(basicPasses);
+		glrenderer->SetPass(edl);
+	}
+	else if(glrenderer->GetPass())
+	{
+		glrenderer->GetPass()->ReleaseGraphicsResources(NULL);
+		glrenderer->SetPass(NULL);
+	}
+
+	this->update();
+#else
+	if(on)
+	{
+		UERROR("RTAB-Map must be built with VTK>=7 to enable EDL shading!");
+	}
+#endif
+}
+
 void CloudViewer::setLighting(bool on)
 {
 	_aSetLighting->setChecked(on);
@@ -2561,7 +2631,7 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 			}
 			pcl::toPCLPointCloud2(*_trajectory, mesh.cloud);
 			mesh.polygons.push_back(vertices);
-			_visualizer->addPolylineFromPolygonMesh(mesh, "trajectory", 1);
+			_visualizer->addPolylineFromPolygonMesh(mesh, "trajectory", 2);
 		}
 
 		if(pose != _lastPose || _lastPose.isNull())
@@ -2686,7 +2756,7 @@ void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector
 				}
 				std::string id = uFormat("reference_frustum_%d", i);
 				this->removeFrustum(id);
-				this->addOrUpdateFrustum(id, pose, baseToCamera, _frustumScale, _frustumColor);
+				this->addOrUpdateFrustum(id, pose, baseToCamera, _frustumScale, _frustumColor, models[i].fovX(), models[i].fovY());
 				if(!baseToCamera.isIdentity())
 				{
 					this->addOrUpdateLine(uFormat("reference_frustum_line_%d", i), pose, pose * baseToCamera, _frustumColor);
@@ -2848,6 +2918,10 @@ bool CloudViewer::isPolygonPicking() const
 {
 	return _aPolygonPicking->isChecked();
 }
+bool CloudViewer::isEDLShadingOn() const
+{
+	return _aSetEDLShading->isChecked();
+}
 bool CloudViewer::isLightingOn() const
 {
 	return _aSetLighting->isChecked();
@@ -2938,14 +3012,17 @@ void CloudViewer::addGrid()
 		{
 			//over x
 			name = uFormat("line%d", ++id);
-			_visualizer->addLine(pcl::PointXYZ(i, min, 0.0f), pcl::PointXYZ(i, max, 0.0f), r, g, b, name, 1);
+			_visualizer->addLine(
+					pcl::PointXYZ(i, min, 0.0f),
+					pcl::PointXYZ(i, max, 0.0f),
+					r, g, b, name, 2);
 			_gridLines.push_back(name);
 			//over y or z
 			name = uFormat("line%d", ++id);
 			_visualizer->addLine(
 					pcl::PointXYZ(min, i, 0),
 					pcl::PointXYZ(max, i, 0),
-					r, g, b, name, 1);
+					r, g, b, name, 2);
 			_gridLines.push_back(name);
 		}
 	}
@@ -3343,7 +3420,7 @@ void CloudViewer::handleAction(QAction * a)
 	else if(a == _aSetGridCellSize)
 	{
 		bool ok;
-		double value = QInputDialog::getDouble(this, tr("Set grid cell size"), tr("Size (m)"), _gridCellSize, 0.01, 10, 2, &ok);
+		double value = QInputDialog::getDouble(this, tr("Set grid cell size"), tr("Size (m)"), _gridCellSize, 0.01, 1000, 2, &ok);
 		if(ok)
 		{
 			this->setGridCellSize(value);
@@ -3415,6 +3492,10 @@ void CloudViewer::handleAction(QAction * a)
 	{
 		this->setCameraOrtho(_aCameraOrtho->isChecked());
 	}
+	else if(a == _aSetEDLShading)
+	{
+		this->setEDLShading(_aSetEDLShading->isChecked());
+	}
 	else if(a == _aSetLighting)
 	{
 		this->setLighting(_aSetLighting->isChecked());

guilib/src/DatabaseViewer.cpp

@@ -205,15 +205,17 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	uInsert(parameters, Parameters::getDefaultParameters("FREAK"));
 	uInsert(parameters, Parameters::getDefaultParameters("BRISK"));
 	uInsert(parameters, Parameters::getDefaultParameters("KAZE"));
-	uInsert(parameters, Parameters::getDefaultParameters("SPTorch"));
+	uInsert(parameters, Parameters::getDefaultParameters("SuperPoint"));
 	uInsert(parameters, Parameters::getDefaultParameters("Optimizer"));
 	uInsert(parameters, Parameters::getDefaultParameters("g2o"));
 	uInsert(parameters, Parameters::getDefaultParameters("GTSAM"));
 	uInsert(parameters, Parameters::getDefaultParameters("Reg"));
 	uInsert(parameters, Parameters::getDefaultParameters("Vis"));
 	uInsert(parameters, Parameters::getDefaultParameters("Icp"));
+	uInsert(parameters, Parameters::getDefaultParameters("PyMatcher"));
 	uInsert(parameters, Parameters::getDefaultParameters("Stereo"));
 	uInsert(parameters, Parameters::getDefaultParameters("StereoBM"));
+	uInsert(parameters, Parameters::getDefaultParameters("StereoSGBM"));
 	uInsert(parameters, Parameters::getDefaultParameters("Grid"));
 	uInsert(parameters, Parameters::getDefaultParameters("GridGlobal"));
 	uInsert(parameters, Parameters::getDefaultParameters("Marker"));
@@ -5117,10 +5119,6 @@ void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
 			{
 				if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
 				{
-					// PINK features
-					ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingFeatureColor());
-					ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingFeatureColor());
-
 					// Add lines
 					// Draw lines between corresponding features...
 					float scaleAX = ui_->graphicsView_A->viewScale();
@@ -5159,6 +5157,13 @@ void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
 					{
 						cA = ui_->graphicsView_A->getDefaultMatchingFeatureColor();
 						cB = ui_->graphicsView_B->getDefaultMatchingFeatureColor();
+						ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingFeatureColor());
+						ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingFeatureColor());
+					}
+					else
+					{
+						ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingLineColor());
+						ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingLineColor());
 					}
 
 					ui_->graphicsView_A->addLine(
@@ -7155,7 +7160,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 
 		if(!silent && ui_->dockWidget_constraints->isVisible())
 		{
-			if(fromS->id() > 0 && toS->id() > 0)
+			if(toS && fromS->id() > 0 && toS->id() > 0)
 			{
 				updateLoopClosuresSlider(fromS->id(), toS->id());
 				if(newLink.type() != Link::kNeighbor && fromS->id() < toS->id())
@@ -7179,9 +7184,16 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 			}
 		}
 	}
-
 	else if(!silent)
 	{
+		if(toS && fromS->id() > 0 && toS->id() > 0)
+		{
+			// just update matches in the views
+			ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
+			ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
+			updateWordsMatching(info.inliersIDs);
+		}
+
 		QMessageBox::warning(this,
 				tr("Refine link"),
 				tr("Cannot find a transformation between nodes %1 and %2: %3").arg(currentLink.from()).arg(currentLink.to()).arg(info.rejectedMsg.c_str()));
@@ -7310,8 +7322,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 										"the database has images, it is recommended to use %2=2 instead so that "
 										"the guess can be found visually.")
 										.arg(Parameters::kRegStrategy().c_str()).arg(Parameters::kRegStrategy().c_str()),
-										QMessageBox::StandardButton::Yes | QMessageBox::StandardButton::No,
-										QMessageBox::StandardButton::Yes) == QMessageBox::StandardButton::Yes)
+										QMessageBox::Yes | QMessageBox::No,
+										QMessageBox::Yes) == QMessageBox::Yes)
 							{
 								guess = fromIter->second.inverse() * toIter->second;
 							}
@@ -7334,8 +7346,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 							"\n\nOtherwise, if the database has images, it is recommended to use %2=2 "
 							"instead so that the guess can be found visually.")
 							.arg(Parameters::kRegStrategy().c_str()).arg(Parameters::kRegStrategy().c_str()),
-							QMessageBox::StandardButton::Yes | QMessageBox::StandardButton::Abort,
-							QMessageBox::StandardButton::Abort) == QMessageBox::StandardButton::Yes)
+							QMessageBox::Yes | QMessageBox::Abort,
+							QMessageBox::Abort) == QMessageBox::Yes)
 				{
 					guess.setIdentity();
 				}

guilib/src/DepthCalibrationDialog.cpp

@@ -445,7 +445,7 @@ void DepthCalibrationDialog::calibrate(
 				for(std::map<int, SensorData>::iterator iter=sequence.begin(); iter!=sequence.end(); ++iter)
 				{
 					Transform baseToCamera = iter->second.cameraModels()[0].localTransform();
-					viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first), baseToCamera, 0.2);
+					viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first), baseToCamera, 0.2, QColor(), iter->second.cameraModels()[0].fovX(), iter->second.cameraModels()[0].fovY());
 				}
 				_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)... done.").arg(map->size()).arg(sequence.size()));
 

guilib/src/GuiLib.qrc

@@ -26,6 +26,7 @@
         <file>images/system-log-out.png</file>
         <file>images/kinect_xbox_360.png</file>
         <file>images/kinect_xbox_one.png</file>
+        <file>images/k4a.png</file>
         <file>images/sense.png</file>
         <file>images/xtion_pro_live.png</file>
         <file>images/bumblebee2.png</file>

guilib/src/ImageView.cpp

@@ -159,7 +159,7 @@ QIcon ImageView::createIcon(const QColor & color)
 ImageView::ImageView(QWidget * parent) :
 		QWidget(parent),
 		_savedFileName((QDir::homePath()+ "/") + "picture" + ".png"),
-		_alpha(50),
+		_alpha(100),
 		_featuresSize(0.0f),
 		_defaultBgColor(Qt::black),
 		_defaultFeatureColor(Qt::yellow),
@@ -467,12 +467,18 @@ void ImageView::setGraphicsViewMode(bool on)
 	{
 		for(QMultiMap<int, KeypointItem*>::iterator iter=_features.begin(); iter!=_features.end(); ++iter)
 		{
-			_graphicsView->scene()->addItem(iter.value());
+			if(iter.value()->scene() != _graphicsView->scene())
+			{
+				_graphicsView->scene()->addItem(iter.value());
+			}
 		}
 
 		for(QList<QGraphicsLineItem*>::iterator iter=_lines.begin(); iter!=_lines.end(); ++iter)
 		{
-			_graphicsView->scene()->addItem(*iter);
+			if((*iter)->scene() != _graphicsView->scene())
+			{
+				_graphicsView->scene()->addItem(*iter);
+			}
 		}
 
 		//update images
@@ -566,15 +572,80 @@ void ImageView::setDefaultBackgroundColor(const QColor & color)
 
 void ImageView::setDefaultFeatureColor(const QColor & color)
 {
+	QColor previousColor = _defaultFeatureColor;
 	_defaultFeatureColor = color;
+	for(QMultiMap<int, KeypointItem*>::iterator iter=_features.begin(); iter!=_features.end(); ++iter)
+	{
+		if(QColor(iter.value()->pen().color().rgb()) == previousColor)
+		{
+			QColor c = _defaultFeatureColor;
+			c.setAlpha(_alpha);
+			iter.value()->setPen(QPen(c));
+			iter.value()->setBrush(QBrush(c));
+		}
+	}
+	if(!_graphicsView->isVisible())
+	{
+		this->update();
+	}
 }
 void ImageView::setDefaultMatchingFeatureColor(const QColor & color)
 {
+	QColor previousColor = _defaultMatchingFeatureColor;
 	_defaultMatchingFeatureColor = color;
+	for(QMultiMap<int, KeypointItem*>::iterator iter=_features.begin(); iter!=_features.end(); ++iter)
+	{
+		if(QColor(iter.value()->pen().color().rgb()) == previousColor)
+		{
+			QColor c = _defaultMatchingFeatureColor;
+			c.setAlpha(_alpha);
+			iter.value()->setPen(QPen(c));
+			iter.value()->setBrush(QBrush(c));
+		}
+	}
+
+	for(QList<QGraphicsLineItem*>::iterator iter=_lines.begin(); iter!=_lines.end(); ++iter)
+	{
+		if(QColor((*iter)->pen().color().rgb()) == previousColor)
+		{
+			QColor c = _defaultMatchingFeatureColor;
+			c.setAlpha(_alpha);
+			(*iter)->setPen(QPen(c));
+		}
+	}
+	if(!_graphicsView->isVisible())
+	{
+		this->update();
+	}
 }
 void ImageView::setDefaultMatchingLineColor(const QColor & color)
 {
+	QColor previousColor = _defaultMatchingLineColor;
 	_defaultMatchingLineColor = color;
+	for(QMultiMap<int, KeypointItem*>::iterator iter=_features.begin(); iter!=_features.end(); ++iter)
+	{
+		if(QColor(iter.value()->pen().color().rgb()) == previousColor)
+		{
+			QColor c = _defaultMatchingLineColor;
+			c.setAlpha(_alpha);
+			iter.value()->setPen(QPen(c));
+			iter.value()->setBrush(QBrush(c));
+		}
+	}
+
+	for(QList<QGraphicsLineItem*>::iterator iter=_lines.begin(); iter!=_lines.end(); ++iter)
+	{
+		if(QColor((*iter)->pen().color().rgb()) == previousColor)
+		{
+			QColor c = _defaultMatchingLineColor;
+			c.setAlpha(_alpha);
+			(*iter)->setPen(QPen(c));
+		}
+	}
+	if(!_graphicsView->isVisible())
+	{
+		this->update();
+	}
 }
 
 void ImageView::setBackgroundColor(const QColor & color)

guilib/src/MainWindow.cpp

@@ -431,6 +431,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
 	connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2()));
 	connect(_ui->actionKinect_for_Windows_SDK_v2, SIGNAL(triggered()), this, SLOT(selectK4W2()));
+	connect(_ui->actionKinect_for_Azure, SIGNAL(triggered()), this, SLOT(selectK4A()));
 	connect(_ui->actionRealSense_R200, SIGNAL(triggered()), this, SLOT(selectRealSense()));
 	connect(_ui->actionRealSense_ZR300, SIGNAL(triggered()), this, SLOT(selectRealSense()));
 	connect(_ui->actionRealSense2_SR300, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
@@ -451,6 +452,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available());
 	_ui->actionFreenect2->setEnabled(CameraFreenect2::available());
 	_ui->actionKinect_for_Windows_SDK_v2->setEnabled(CameraK4W2::available());
+	_ui->actionKinect_for_Azure->setEnabled(CameraK4A::available());
 	_ui->actionRealSense_R200->setEnabled(CameraRealSense::available());
 	_ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available());
 	_ui->actionRealSense2_SR300->setEnabled(CameraRealSense2::available());
@@ -603,6 +605,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->statsToolBox->updateStat("Odometry/VarianceAng/", false);
 	_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false);
 	_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", false);
+	_ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", false);
+	_ui->statsToolBox->updateStat("Odometry/GravityPitchError/deg", false);
 	_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", false);
 	_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", false);
 	_ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", false);
@@ -1272,7 +1276,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 						if(!t.isNull())
 						{
 							QColor color = Qt::yellow;
-							_cloudViewer->addOrUpdateFrustum(frustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color);
+							_cloudViewer->addOrUpdateFrustum(frustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
 						}
 					}
 				}
@@ -1338,11 +1342,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 	{
 		_odometryReceived = true;
 		// update camera position
-		if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
+		if(odom.data().cameraModels().size() && odom.data().cameraModels()[0].isValidForProjection())
 		{
 			_cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), odom.data().cameraModels());
 		}
-		else if(!odom.data().stereoCameraModel().localTransform().isNull())
+		else if(odom.data().stereoCameraModel().isValidForProjection())
 		{
 			_cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), odom.data().stereoCameraModel());
 		}
@@ -1522,14 +1526,25 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 	_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
+	if(odom.info().timeParticleFiltering>0.0f)
+	{
+		_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
+	}
+	if(odom.info().gravityRollError>0.0f || odom.info().gravityPitchError > 0.0f)
+	{
+		_ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().gravityRollError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
+		_ui->statsToolBox->updateStat("Odometry/GravityPitchError/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().gravityPitchError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
+	}
 	_ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localMapSize, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localScanMapSize, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localKeyFrames, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
+	if(odom.info().localBundleTime > 0.0f)
+	{
+		_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
+		_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
+		_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
+	}
 	_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.data().id(), _preferencesDialog->isCacheSavedInFigures());
 
@@ -1941,9 +1956,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 			UDEBUG("time= %d ms", time.restart());
 
-			_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), signature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
-			_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), loopSignature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
-
 			// loop closure view
 			if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0)  &&
 			   !stat.loopClosureTransform().isNull() &&
@@ -2061,11 +2073,11 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			{
 				if(poses.rbegin()->first == stat.getLastSignatureData().id())
 				{
-					if(stat.getLastSignatureData().sensorData().cameraModels().size() && !stat.getLastSignatureData().sensorData().cameraModels()[0].localTransform().isNull())
+					if(stat.getLastSignatureData().sensorData().cameraModels().size() && stat.getLastSignatureData().sensorData().cameraModels()[0].isValidForProjection())
 					{
 						_cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().cameraModels());
 					}
-					else if(!stat.getLastSignatureData().sensorData().stereoCameraModel().localTransform().isNull())
+					else if(stat.getLastSignatureData().sensorData().stereoCameraModel().isValidForProjection())
 					{
 						_cloudViewer->updateCameraFrustum(poses.rbegin()->second, stat.getLastSignatureData().sensorData().stereoCameraModel());
 					}
@@ -2644,17 +2656,18 @@ void MainWindow::updateMapCloud(
 					// Supporting only one frustum per node
 					if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection())
 					{
-						Transform t = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().localTransform():s.sensorData().cameraModels()[0].localTransform();
+						const CameraModel & model = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().left():s.sensorData().cameraModels()[0];
+						Transform t = model.localTransform();
 						if(!t.isNull())
 						{
 							QColor color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
-							_cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color);
+							_cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
 
 							if(_currentGTPosesMap.find(iter->first)!=_currentGTPosesMap.end())
 							{
 								std::string gtFrustumId = uFormat("f_gt_%d", iter->first);
 								color = Qt::gray;
-								_cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color);
+								_cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
 							}
 						}
 					}
@@ -4817,7 +4830,7 @@ void MainWindow::newDatabase()
 
 void MainWindow::openDatabase()
 {
-	QString path = QFileDialog::getOpenFileName(this, tr("Open database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
+	QString path = QFileDialog::getOpenFileName(this, tr("Open database..."), _defaultOpenDatabasePath.isEmpty()?_preferencesDialog->getWorkingDirectory():_defaultOpenDatabasePath, tr("RTAB-Map database files (*.db)"));
 	if(!path.isEmpty())
 	{
 		this->openDatabase(path);
@@ -4843,6 +4856,7 @@ void MainWindow::openDatabase(const QString & path, const ParametersMap & overri
 
 		this->clearTheCache();
 		_openedDatabasePath = path;
+		_defaultOpenDatabasePath = path;
 
 		// look if there are saved parameters
 		DBDriver * driver = DBDriver::create();
@@ -5203,6 +5217,11 @@ void MainWindow::startDetection()
 				odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
 				int odomStrategy = Parameters::defaultOdomStrategy();
 				Parameters::parse(odomParameters, Parameters::kOdomStrategy(), odomStrategy);
+				double gravitySigma = _preferencesDialog->getOdomF2MGravitySigma();
+				if(gravitySigma >= 0.0)
+				{
+					uInsert(odomParameters, ParametersPair(Parameters::kOptimizerGravitySigma(), uNumber2Str(gravitySigma)));
+				}
 				if(odomStrategy != 1)
 				{
 					// Only Frame To Frame supports all VisCorType
@@ -6331,6 +6350,11 @@ void MainWindow::selectK4W2()
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcK4W2);
 }
 
+void MainWindow::selectK4A()
+{
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcK4A);
+}
+
 void MainWindow::selectRealSense()
 {
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense);

guilib/src/ParametersToolBox.cpp

@@ -521,7 +521,9 @@ void ParametersToolBox::addParameter(QVBoxLayout * layout, const QString & key,
 	QString tmp = key.split('/').last();
 	QLabel * label = new QLabel(tmp, this);
 	label->setObjectName(key+"/label");
-	label->setToolTip(QString("<FONT>%1</FONT>").arg(Parameters::getDescription(key.toStdString()).c_str()));
+	label->setToolTip(QString("<FONT>%1 [default=%2]</FONT>")
+			.arg(Parameters::getDescription(key.toStdString()).c_str())
+			.arg(uValue(Parameters::getDefaultParameters(), key.toStdString(), std::string("?")).c_str()));
 	label->setTextInteractionFlags(Qt::TextSelectableByMouse);
 	hLayout->addWidget(label);
 	hLayout->addWidget(widget);

guilib/src/PreferencesDialog.cpp

@@ -146,9 +146,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->checkBox_ORBGpu->setEnabled(false);
 		_ui->label_orbGpu->setEnabled(false);
 
-		// remove BruteForceGPU option
-		_ui->comboBox_dictionary_strategy->removeItem(4);
-		_ui->reextract_nn->removeItem(4);
+		// disable BruteForceGPU option
+		_ui->comboBox_dictionary_strategy->setItemData(4, 0, Qt::UserRole - 1);
+		_ui->reextract_nn->setItemData(4, 0, Qt::UserRole - 1);
 	}
 
 #ifndef RTABMAP_OCTOMAP
@@ -191,26 +191,29 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->stereosgbm_mode->setItemData(2, 0, Qt::UserRole - 1);
 #endif
 
+//SURF
 #ifndef RTABMAP_NONFREE
-		_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
-		_ui->comboBox_detector_strategy->setItemData(1, 0, Qt::UserRole - 1);
-		_ui->vis_feature_detector->setItemData(0, 0, Qt::UserRole - 1);
-		_ui->vis_feature_detector->setItemData(1, 0, Qt::UserRole - 1);
-
-#if CV_MAJOR_VERSION >= 3
-		_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
-		_ui->comboBox_detector_strategy->setItemData(1, 0, Qt::UserRole - 1);
-		_ui->comboBox_detector_strategy->setItemData(3, 0, Qt::UserRole - 1);
-		_ui->comboBox_detector_strategy->setItemData(4, 0, Qt::UserRole - 1);
-		_ui->comboBox_detector_strategy->setItemData(5, 0, Qt::UserRole - 1);
-		_ui->comboBox_detector_strategy->setItemData(6, 0, Qt::UserRole - 1);
-		_ui->vis_feature_detector->setItemData(0, 0, Qt::UserRole - 1);
-		_ui->vis_feature_detector->setItemData(1, 0, Qt::UserRole - 1);
-		_ui->vis_feature_detector->setItemData(3, 0, Qt::UserRole - 1);
-		_ui->vis_feature_detector->setItemData(4, 0, Qt::UserRole - 1);
-		_ui->vis_feature_detector->setItemData(5, 0, Qt::UserRole - 1);
-		_ui->vis_feature_detector->setItemData(6, 0, Qt::UserRole - 1);
+	_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
+	_ui->vis_feature_detector->setItemData(0, 0, Qt::UserRole - 1);
 #endif
+
+// SIFT
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
+#ifndef RTABMAP_NONFREE
+	_ui->comboBox_detector_strategy->setItemData(1, 0, Qt::UserRole - 1);
+	_ui->vis_feature_detector->setItemData(1, 0, Qt::UserRole - 1);
+#endif
+#endif
+
+#if CV_MAJOR_VERSION >= 3 && !defined(HAVE_OPENCV_XFEATURES2D)
+	_ui->comboBox_detector_strategy->setItemData(3, 0, Qt::UserRole - 1);
+	_ui->comboBox_detector_strategy->setItemData(4, 0, Qt::UserRole - 1);
+	_ui->comboBox_detector_strategy->setItemData(5, 0, Qt::UserRole - 1);
+	_ui->comboBox_detector_strategy->setItemData(6, 0, Qt::UserRole - 1);
+	_ui->vis_feature_detector->setItemData(3, 0, Qt::UserRole - 1);
+	_ui->vis_feature_detector->setItemData(4, 0, Qt::UserRole - 1);
+	_ui->vis_feature_detector->setItemData(5, 0, Qt::UserRole - 1);
+	_ui->vis_feature_detector->setItemData(6, 0, Qt::UserRole - 1);
 #endif
 
 #ifndef RTABMAP_ORB_OCTREE
@@ -218,9 +221,17 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->vis_feature_detector->setItemData(10, 0, Qt::UserRole - 1);
 #endif
 
-#ifndef RTABMAP_SP_TORCH
-		_ui->comboBox_detector_strategy->setItemData(11, 0, Qt::UserRole - 1);
-		_ui->vis_feature_detector->setItemData(11, 0, Qt::UserRole - 1);
+#ifndef RTABMAP_SUPERPOINT_TORCH
+	_ui->comboBox_detector_strategy->setItemData(11, 0, Qt::UserRole - 1);
+	_ui->vis_feature_detector->setItemData(11, 0, Qt::UserRole - 1);
+#endif
+
+#ifndef RTABMAP_PYMATCHER
+	_ui->reextract_nn->setItemData(6, 0, Qt::UserRole - 1);
+#endif
+
+#if !defined(HAVE_OPENCV_XFEATURES2D) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION<4 || CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<1))
+	_ui->reextract_nn->setItemData(7, 0, Qt::UserRole - 1);
 #endif
 
 #if CV_MAJOR_VERSION >= 3
@@ -410,6 +421,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->radioButton_nochangeGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkbox_odomDisabled, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->odom_registration, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
+	connect(_ui->odom_f2m_gravitySigma, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkbox_groundTruthAlign, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 
 	// Cloud rendering panel
@@ -722,6 +734,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	connect(_ui->checkbox_stereoMyntEye_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_stereoMyntEye_depth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkbox_stereoMyntEye_autoExposure, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_stereoMyntEye_gain, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_stereoMyntEye_brightness, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_stereoMyntEye_contrast, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_stereoMyntEye_irControl, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
 	connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -863,6 +880,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->surf_doubleSpinBox_maxDepth->setObjectName(Parameters::kKpMaxDepth().c_str());
 	_ui->surf_doubleSpinBox_minDepth->setObjectName(Parameters::kKpMinDepth().c_str());
 	_ui->checkBox_memDepthAsMask->setObjectName(Parameters::kMemDepthAsMask().c_str());
+	_ui->checkBox_memStereoFromMotion->setObjectName(Parameters::kMemStereoFromMotion().c_str());
 	_ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpMaxFeatures().c_str());
 	_ui->spinBox_KPGridRows->setObjectName(Parameters::kKpGridRows().c_str());
 	_ui->spinBox_KPGridCols->setObjectName(Parameters::kKpGridCols().c_str());
@@ -896,6 +914,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->sift_doubleSpinBox_contrastThr->setObjectName(Parameters::kSIFTContrastThreshold().c_str());
 	_ui->sift_doubleSpinBox_edgeThr->setObjectName(Parameters::kSIFTEdgeThreshold().c_str());
 	_ui->sift_doubleSpinBox_sigma->setObjectName(Parameters::kSIFTSigma().c_str());
+	_ui->sift_checkBox_rootsift->setObjectName(Parameters::kSIFTRootSIFT().c_str());
 
 	//BRIEF descriptor
 	_ui->briefBytes->setObjectName(Parameters::kBRIEFBytes().c_str());
@@ -948,12 +967,26 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_kaze_diffusivity->setObjectName(Parameters::kKAZEDiffusivity().c_str());
 
 	// SuperPoint Torch
-	_ui->lineEdit_sptorch_path->setObjectName(Parameters::kSPTorchModelPath().c_str());
-	connect(_ui->toolButton_sptorch_path, SIGNAL(clicked()), this, SLOT(changeSPTorchModelPath()));
-	_ui->doubleSpinBox_sptorch_threshold->setObjectName(Parameters::kSPTorchThreshold().c_str());
-	_ui->checkBox_sptorch_nms->setObjectName(Parameters::kSPTorchNMS().c_str());
-	_ui->spinBox_sptorch_minDistance->setObjectName(Parameters::kSPTorchMinDistance().c_str());
-	_ui->checkBox_sptorch_cuda->setObjectName(Parameters::kSPTorchCuda().c_str());
+	_ui->lineEdit_sptorch_path->setObjectName(Parameters::kSuperPointModelPath().c_str());
+	connect(_ui->toolButton_sptorch_path, SIGNAL(clicked()), this, SLOT(changeSuperPointModelPath()));
+	_ui->doubleSpinBox_sptorch_threshold->setObjectName(Parameters::kSuperPointThreshold().c_str());
+	_ui->checkBox_sptorch_nms->setObjectName(Parameters::kSuperPointNMS().c_str());
+	_ui->spinBox_sptorch_minDistance->setObjectName(Parameters::kSuperPointNMSRadius().c_str());
+	_ui->checkBox_sptorch_cuda->setObjectName(Parameters::kSuperPointCuda().c_str());
+
+	// PyMatcher
+	_ui->lineEdit_pymatcher_path->setObjectName(Parameters::kPyMatcherPath().c_str());
+	connect(_ui->toolButton_pymatcher_path, SIGNAL(clicked()), this, SLOT(changePyMatcherPath()));
+	_ui->pymatcher_matchThreshold->setObjectName(Parameters::kPyMatcherThreshold().c_str());
+	_ui->pymatcher_iterations->setObjectName(Parameters::kPyMatcherIterations().c_str());
+	_ui->checkBox_pymatcher_cuda->setObjectName(Parameters::kPyMatcherCuda().c_str());
+	_ui->lineEdit_pymatcher_model->setObjectName(Parameters::kPyMatcherModel().c_str());
+	connect(_ui->toolButton_pymatcher_model, SIGNAL(clicked()), this, SLOT(changePyMatcherPath()));
+
+	// GMS
+	_ui->checkBox_gms_withRotation->setObjectName(Parameters::kGMSWithRotation().c_str());
+	_ui->checkBox_gms_withScale->setObjectName(Parameters::kGMSWithScale().c_str());
+	_ui->gms_thresholdFactor->setObjectName(Parameters::kGMSThresholdFactor().c_str());
 
 	// verifyHypotheses
 	_ui->groupBox_vh_epipolar2->setObjectName(Parameters::kVhEpEnabled().c_str());
@@ -1009,6 +1042,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->localDetection_maxPaths->setObjectName(Parameters::kRGBDProximityMaxPaths().c_str());
 	_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDProximityPathFilteringRadius().c_str());
 	_ui->localDetection_angle->setObjectName(Parameters::kRGBDProximityAngle().c_str());
+	_ui->checkBox_localSpaceOdomGuess->setObjectName(Parameters::kRGBDProximityOdomGuess().c_str());
 	_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDProximityPathRawPosesUsed().c_str());
 	_ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str());
 	_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
@@ -1038,8 +1072,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
 	_ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str());
 	_ui->reextract_nn->setObjectName(Parameters::kVisCorNNType().c_str());
+	connect(_ui->reextract_nn, SIGNAL(currentIndexChanged(int)), this, SLOT(updateFeatureMatchingVisibility()));
 	_ui->reextract_nndrRatio->setObjectName(Parameters::kVisCorNNDR().c_str());
-	_ui->checkBox__visCorCrossCheck->setObjectName(Parameters::kVisCorCrossCheck().c_str());
 	_ui->spinBox_visCorGuessWinSize->setObjectName(Parameters::kVisCorGuessWinSize().c_str());
 	_ui->checkBox__visCorGuessMatchToProjection->setObjectName(Parameters::kVisCorGuessMatchToProjection().c_str());
 	_ui->vis_feature_detector->setObjectName(Parameters::kVisFeatureType().c_str());
@@ -1882,6 +1916,11 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkbox_stereoRealSense2_odom->setChecked(false);
 		_ui->checkbox_stereoMyntEye_rectify->setChecked(false);
 		_ui->checkbox_stereoMyntEye_depth->setChecked(false);
+		_ui->checkbox_stereoMyntEye_autoExposure->setChecked(true);
+		_ui->spinBox_stereoMyntEye_gain->setValue(24);
+		_ui->spinBox_stereoMyntEye_brightness->setValue(120);
+		_ui->spinBox_stereoMyntEye_contrast->setValue(116);
+		_ui->spinBox_stereoMyntEye_irControl->setValue(0);
 
 		_ui->checkBox_cameraImages_timestamps->setChecked(false);
 		_ui->checkBox_cameraImages_syncTimeStamps->setChecked(true);
@@ -1993,6 +2032,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		if(groupBox->objectName() == _ui->groupBox_odometry1->objectName())
 		{
 			_ui->odom_registration->setCurrentIndex(3);
+			_ui->odom_f2m_gravitySigma->setValue(-1);
 		}
 	}
 }
@@ -2118,6 +2158,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->radioButton_nochangeGraphView->setChecked(settings.value("nochangeGraphView", _ui->radioButton_nochangeGraphView->isChecked()).toBool());
 	_ui->checkbox_odomDisabled->setChecked(settings.value("odomDisabled", _ui->checkbox_odomDisabled->isChecked()).toBool());
 	_ui->odom_registration->setCurrentIndex(settings.value("odomRegistration", _ui->odom_registration->currentIndex()).toInt());
+	_ui->odom_f2m_gravitySigma->setValue(settings.value("odomF2MGravitySigma", _ui->odom_f2m_gravitySigma->value()).toDouble());
 	_ui->checkbox_groundTruthAlign->setChecked(settings.value("gtAlign", _ui->checkbox_groundTruthAlign->isChecked()).toBool());
 
 	for(int i=0; i<2; ++i)
@@ -2330,7 +2371,12 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	settings.beginGroup("MyntEye");
 	_ui->checkbox_stereoMyntEye_rectify->setChecked(settings.value("rectify", _ui->checkbox_stereoMyntEye_rectify->isChecked()).toBool());
 	_ui->checkbox_stereoMyntEye_depth->setChecked(settings.value("depth", _ui->checkbox_stereoMyntEye_depth->isChecked()).toBool());
-	settings.endGroup(); // StereoRealSense2
+	_ui->checkbox_stereoMyntEye_autoExposure->setChecked(settings.value("auto_exposure", _ui->checkbox_stereoMyntEye_autoExposure->isChecked()).toBool());
+	_ui->spinBox_stereoMyntEye_gain->setValue(settings.value("gain", _ui->spinBox_stereoMyntEye_gain->value()).toInt());
+	_ui->spinBox_stereoMyntEye_brightness->setValue(settings.value("brightness", _ui->spinBox_stereoMyntEye_brightness->value()).toInt());
+	_ui->spinBox_stereoMyntEye_contrast->setValue(settings.value("contrast", _ui->spinBox_stereoMyntEye_contrast->value()).toInt());
+	_ui->spinBox_stereoMyntEye_irControl->setValue(settings.value("ir_control", _ui->spinBox_stereoMyntEye_irControl->value()).toInt());
+	settings.endGroup(); // MyntEye
 
 	settings.beginGroup("Images");
 	_ui->source_images_lineEdit_path->setText(settings.value("path", _ui->source_images_lineEdit_path->text()).toString());
@@ -2588,6 +2634,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("nochangeGraphView",    _ui->radioButton_nochangeGraphView->isChecked());
 	settings.setValue("odomDisabled",         _ui->checkbox_odomDisabled->isChecked());
 	settings.setValue("odomRegistration",     _ui->odom_registration->currentIndex());
+	settings.setValue("odomF2MGravitySigma",  _ui->odom_f2m_gravitySigma->value());
 	settings.setValue("gtAlign",              _ui->checkbox_groundTruthAlign->isChecked());
 
 	for(int i=0; i<2; ++i)
@@ -2800,8 +2847,13 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.endGroup(); // StereoRealSense2
 
 	settings.beginGroup("MyntEye");
-	settings.setValue("rectify", _ui->checkbox_stereoMyntEye_rectify->isChecked());
-	settings.setValue("depth", _ui->checkbox_stereoMyntEye_depth->isChecked());
+	settings.setValue("rectify",       _ui->checkbox_stereoMyntEye_rectify->isChecked());
+	settings.setValue("depth",         _ui->checkbox_stereoMyntEye_depth->isChecked());
+	settings.setValue("auto_exposure", _ui->checkbox_stereoMyntEye_autoExposure->isChecked());
+	settings.setValue("gain",          _ui->spinBox_stereoMyntEye_gain->value());
+	settings.setValue("brightness",    _ui->spinBox_stereoMyntEye_brightness->value());
+	settings.setValue("contrast",      _ui->spinBox_stereoMyntEye_contrast->value());
+	settings.setValue("ir_control",    _ui->spinBox_stereoMyntEye_irControl->value());
 	settings.endGroup(); // MyntEye
 
 	settings.beginGroup("Images");
@@ -2893,6 +2945,7 @@ void PreferencesDialog::writeCoreSettings(const QString & filePath) const
 
 bool PreferencesDialog::validateForm()
 {
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION < 3 || (CV_MINOR_VERSION==3 && !defined(RTABMAP_OPENCV_DEV))))
 #ifndef RTABMAP_NONFREE
 	// verify that SURF/SIFT cannot be selected if not built with OpenCV nonfree module
 	// BOW dictionary type
@@ -2913,6 +2966,28 @@ bool PreferencesDialog::validateForm()
 		_ui->vis_feature_detector->setCurrentIndex(Feature2D::kFeatureFastBrief);
 	}
 #endif
+#else //>= 4.3.0-dev
+#ifndef RTABMAP_NONFREE
+	// verify that SURF cannot be selected if not built with OpenCV nonfree module
+	// BOW dictionary type
+	if(_ui->comboBox_detector_strategy->currentIndex() <= 1)
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Selected feature type (SURF) is not available. RTAB-Map is not built "
+				   "with the nonfree module from OpenCV. SIFT is set instead for the bag-of-words dictionary."));
+		_ui->comboBox_detector_strategy->setCurrentIndex(Feature2D::kFeatureSift);
+	}
+	// BOW Reextract features type
+	if(_ui->vis_feature_detector->currentIndex() <= 1)
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Selected feature type (SURF) is not available. RTAB-Map is not built "
+				   "with the nonfree module from OpenCV. Fast/Brief is set instead for the re-extraction "
+				   "of features on loop closure."));
+		_ui->vis_feature_detector->setCurrentIndex(Feature2D::kFeatureFastBrief);
+	}
+#endif
+#endif
 
 #if CV_MAJOR_VERSION < 3
 	if (_ui->comboBox_detector_strategy->currentIndex() == Feature2D::kFeatureKaze)
@@ -4090,7 +4165,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 						(combo->objectName().toStdString().compare(Parameters::kKpDetectorStrategy()) == 0 ||
 						 combo->objectName().toStdString().compare(Parameters::kVisFeatureType()) == 0))
 				{
-					UWARN("Trying to set \"%s\" to SIFT/SURF but RTAB-Map isn't built "
+					UWARN("Trying to set \"%s\" to SURF but RTAB-Map isn't built "
 						  "with the nonfree module from OpenCV. Keeping default combo value: %s.",
 						  combo->objectName().toStdString().c_str(),
 						  combo->currentText().toStdString().c_str());
@@ -4592,6 +4667,12 @@ void PreferencesDialog::updateStereoDisparityVisibility()
 	_ui->label_stereo_rectify->setVisible(_ui->checkBox_stereo_rectify->isEnabled());
 }
 
+void PreferencesDialog::updateFeatureMatchingVisibility()
+{
+	_ui->groupBox_pymatcher->setVisible(_ui->reextract_nn->currentIndex() == 6);
+	_ui->groupBox_gms->setVisible(_ui->reextract_nn->currentIndex() == 7);
+}
+
 void PreferencesDialog::useOdomFeatures()
 {
 	if(this->isVisible() && _ui->checkBox_useOdomFeatures->isChecked())
@@ -4711,7 +4792,7 @@ void PreferencesDialog::changeIcpPMConfigPath()
 	}
 }
 
-void PreferencesDialog::changeSPTorchModelPath()
+void PreferencesDialog::changeSuperPointModelPath()
 {
 	QString path;
 	if(_ui->lineEdit_sptorch_path->text().isEmpty())
@@ -4728,6 +4809,40 @@ void PreferencesDialog::changeSPTorchModelPath()
 	}
 }
 
+void PreferencesDialog::changePyMatcherPath()
+{
+	QString path;
+	if(_ui->lineEdit_pymatcher_path->text().isEmpty())
+	{
+		path = QFileDialog::getOpenFileName(this, tr("Select file"), this->getWorkingDirectory(), tr("Python wrapper (*.py)"));
+	}
+	else
+	{
+		path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_pymatcher_path->text(), tr("Python wrapper (*.py)"));
+	}
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_pymatcher_path->setText(path);
+	}
+}
+
+void PreferencesDialog::changePyMatcherModel()
+{
+	QString path;
+	if(_ui->lineEdit_pymatcher_model->text().isEmpty())
+	{
+		path = QFileDialog::getOpenFileName(this, tr("Select file"), this->getWorkingDirectory(), tr("PyTorch model (*.pth, *.pt)"));
+	}
+	else
+	{
+		path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_pymatcher_model->text(), tr("PyTorch model (*.pth, *.pt)"));
+	}
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_pymatcher_model->setText(path);
+	}
+}
+
 void PreferencesDialog::updateSourceGrpVisibility()
 {
 	_ui->groupBox_sourceRGBD->setVisible(_ui->comboBox_sourceType->currentIndex() == 0);
@@ -4792,6 +4907,7 @@ void PreferencesDialog::updateSourceGrpVisibility()
 			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD) ||
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo) ||
 			(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB) ||
+			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect - kSrcRGBD) || //Kinect360
 			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4A - kSrcRGBD) || //K4A
 			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense2 - kSrcRGBD) || //D435i
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo) || //T265
@@ -4901,6 +5017,10 @@ int PreferencesDialog::getOdomRegistrationApproach() const
 {
 	return _ui->odom_registration->currentIndex();
 }
+double PreferencesDialog::getOdomF2MGravitySigma() const
+{
+	return _ui->odom_f2m_gravitySigma->value();
+}
 bool PreferencesDialog::isGroundTruthAligned() const
 {
 	return _ui->checkbox_groundTruthAlign->isChecked();
@@ -5586,6 +5706,19 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
 			((CameraMyntEye*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked());
+			if(_ui->checkbox_stereoMyntEye_autoExposure->isChecked())
+			{
+				((CameraMyntEye*)camera)->setAutoExposure();
+			}
+			else
+			{
+				((CameraMyntEye*)camera)->setManualExposure(
+						_ui->spinBox_stereoMyntEye_gain->value(),
+						_ui->spinBox_stereoMyntEye_brightness->value(),
+						_ui->spinBox_stereoMyntEye_contrast->value());
+			}
+			((CameraMyntEye*)camera)->setIrControl(
+					_ui->spinBox_stereoMyntEye_irControl->value());
 		}
 	}
 	else if(driver == kSrcRGBDImages)

guilib/src/ui/aboutDialog.ui

@@ -164,7 +164,7 @@ p, li { white-space: pre-wrap; }
              <x>0</x>
              <y>0</y>
              <width>596</width>
-             <height>820</height>
+             <height>843</height>
             </rect>
            </property>
            <layout class="QVBoxLayout" name="verticalLayout_2">
@@ -185,7 +185,7 @@ p, li { white-space: pre-wrap; }
             </property>
             <item>
              <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1">
-              <item row="15" column="0">
+              <item row="16" column="0">
                <widget class="QLabel" name="label_21">
                 <property name="text">
                  <string>With stereo Zed :</string>
@@ -195,7 +195,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="28" column="0">
+              <item row="29" column="0">
                <widget class="QLabel" name="label_25">
                 <property name="text">
                  <string>With FOVIS :</string>
@@ -205,7 +205,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="28" column="2">
+              <item row="29" column="2">
                <widget class="QLabel" name="label_fovis_license">
                 <property name="text">
                  <string>GPLv2</string>
@@ -238,7 +238,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="12" column="1">
+              <item row="13" column="1">
                <widget class="QLabel" name="label_realsense2">
                 <property name="text">
                  <string/>
@@ -251,7 +251,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="32" column="0">
+              <item row="33" column="0">
                <widget class="QLabel" name="label_35">
                 <property name="text">
                  <string>With OKVIS :</string>
@@ -261,7 +261,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="30" column="1">
+              <item row="31" column="1">
                <widget class="QLabel" name="label_dvo">
                 <property name="text">
                  <string/>
@@ -274,7 +274,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="9" column="1">
+              <item row="10" column="1">
                <widget class="QLabel" name="label_openni2">
                 <property name="text">
                  <string/>
@@ -297,7 +297,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="12" column="2">
+              <item row="13" column="2">
                <widget class="QLabel" name="label_realsense2_license">
                 <property name="text">
                  <string>Apache-2</string>
@@ -307,7 +307,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="32" column="1">
+              <item row="33" column="1">
                <widget class="QLabel" name="label_okvis">
                 <property name="text">
                  <string/>
@@ -320,7 +320,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="34" column="0">
+              <item row="35" column="0">
                <widget class="QLabel" name="label_34">
                 <property name="text">
                  <string>With MSCKF :</string>
@@ -330,7 +330,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="24" column="1">
+              <item row="25" column="1">
                <widget class="QLabel" name="label_octomap">
                 <property name="text">
                  <string/>
@@ -343,7 +343,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="34" column="1">
+              <item row="35" column="1">
                <widget class="QLabel" name="label_msckf">
                 <property name="text">
                  <string/>
@@ -356,7 +356,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="9" column="2">
+              <item row="10" column="2">
                <widget class="QLabel" name="label_openni2_license">
                 <property name="text">
                  <string>Apache v2</string>
@@ -376,7 +376,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="16" column="0">
+              <item row="17" column="0">
                <widget class="QLabel" name="label_38">
                 <property name="text">
                  <string>With K4W2 :</string>
@@ -386,7 +386,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="15" column="1">
+              <item row="16" column="1">
                <widget class="QLabel" name="label_zed">
                 <property name="text">
                  <string/>
@@ -399,7 +399,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="25" column="0">
+              <item row="26" column="0">
                <widget class="QLabel" name="label_24">
                 <property name="text">
                  <string>With CPU-TSDF :</string>
@@ -409,7 +409,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="13" column="0">
+              <item row="14" column="0">
                <widget class="QLabel" name="label_16">
                 <property name="text">
                  <string>With stereo dc1394 :</string>
@@ -419,7 +419,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="29" column="0">
+              <item row="30" column="0">
                <widget class="QLabel" name="label_26">
                 <property name="text">
                  <string>With Viso2 :</string>
@@ -429,7 +429,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="31" column="0">
+              <item row="32" column="0">
                <widget class="QLabel" name="label_28">
                 <property name="text">
                  <string>With ORB SLAM 2 :</string>
@@ -449,7 +449,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="31" column="1">
+              <item row="32" column="1">
                <widget class="QLabel" name="label_orbslam2">
                 <property name="text">
                  <string/>
@@ -462,7 +462,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="13" column="2">
+              <item row="14" column="2">
                <widget class="QLabel" name="label_dc1394_license">
                 <property name="text">
                  <string>LGPL</string>
@@ -472,7 +472,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="29" column="2">
+              <item row="30" column="2">
                <widget class="QLabel" name="label_viso2_license">
                 <property name="text">
                  <string>GPLv3</string>
@@ -482,7 +482,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="14" column="0">
+              <item row="15" column="0">
                <widget class="QLabel" name="label_17">
                 <property name="text">
                  <string>With stereo FlyCapture2 :</string>
@@ -492,7 +492,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="24" column="0">
+              <item row="25" column="0">
                <widget class="QLabel" name="label_20">
                 <property name="text">
                  <string>With Octomap :</string>
@@ -502,7 +502,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="18" column="0">
+              <item row="19" column="0">
                <widget class="QLabel" name="label_77">
                 <property name="text">
                  <string>With TORO :</string>
@@ -512,7 +512,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="28" column="1">
+              <item row="29" column="1">
                <widget class="QLabel" name="label_fovis">
                 <property name="text">
                  <string/>
@@ -525,7 +525,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="24" column="2">
+              <item row="25" column="2">
                <widget class="QLabel" name="label_octomap_license">
                 <property name="text">
                  <string>BSD</string>
@@ -535,7 +535,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="10" column="0">
+              <item row="11" column="0">
                <widget class="QLabel" name="label_15">
                 <property name="text">
                  <string>With Freenect2 :</string>
@@ -545,7 +545,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="14" column="1">
+              <item row="15" column="1">
                <widget class="QLabel" name="label_flycapture2">
                 <property name="text">
                  <string/>
@@ -568,7 +568,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="10" column="2">
+              <item row="11" column="2">
                <widget class="QLabel" name="label_freenect2_license">
                 <property name="text">
                  <string>Apache v2 and/or GPLv2</string>
@@ -578,7 +578,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="25" column="1">
+              <item row="26" column="1">
                <widget class="QLabel" name="label_cputsdf">
                 <property name="text">
                  <string/>
@@ -591,7 +591,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="11" column="0">
+              <item row="12" column="0">
                <widget class="QLabel" name="label_22">
                 <property name="text">
                  <string>With RealSense :</string>
@@ -601,7 +601,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="21" column="0">
+              <item row="22" column="0">
                <widget class="QLabel" name="label_18">
                 <property name="text">
                  <string>With cvsba :</string>
@@ -611,7 +611,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="19" column="0">
+              <item row="20" column="0">
                <widget class="QLabel" name="label_14">
                 <property name="text">
                  <string>With g2o :</string>
@@ -621,7 +621,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="21" column="1">
+              <item row="22" column="1">
                <widget class="QLabel" name="label_cvsba">
                 <property name="text">
                  <string/>
@@ -634,7 +634,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="8" column="2">
+              <item row="9" column="2">
                <widget class="QLabel" name="label_freenect_license">
                 <property name="text">
                  <string>Apache v2 and/or GPLv2</string>
@@ -644,7 +644,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="20" column="1">
+              <item row="21" column="1">
                <widget class="QLabel" name="label_gtsam">
                 <property name="text">
                  <string/>
@@ -670,7 +670,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="20" column="0">
+              <item row="21" column="0">
                <widget class="QLabel" name="label_19">
                 <property name="text">
                  <string>With GTSAM :</string>
@@ -680,7 +680,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="13" column="1">
+              <item row="14" column="1">
                <widget class="QLabel" name="label_dc1394">
                 <property name="text">
                  <string/>
@@ -703,7 +703,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="8" column="0">
+              <item row="9" column="0">
                <widget class="QLabel" name="label_12">
                 <property name="text">
                  <string>With Freenect :</string>
@@ -713,7 +713,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="8" column="1">
+              <item row="9" column="1">
                <widget class="QLabel" name="label_freenect">
                 <property name="text">
                  <string/>
@@ -736,7 +736,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="23" column="0">
+              <item row="24" column="0">
                <widget class="QLabel" name="label_29">
                 <property name="text">
                  <string>With libpointmatcher :</string>
@@ -746,7 +746,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="19" column="1">
+              <item row="20" column="1">
                <widget class="QLabel" name="label_g2o">
                 <property name="text">
                  <string/>
@@ -772,7 +772,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="10" column="1">
+              <item row="11" column="1">
                <widget class="QLabel" name="label_freenect2">
                 <property name="text">
                  <string/>
@@ -785,7 +785,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="12" column="0">
+              <item row="13" column="0">
                <widget class="QLabel" name="label_33">
                 <property name="text">
                  <string>With RealSense2 :</string>
@@ -795,7 +795,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="34" column="2">
+              <item row="35" column="2">
                <widget class="QLabel" name="label_msckf_license">
                 <property name="text">
                  <string>Penn Software License</string>
@@ -805,7 +805,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="11" column="1">
+              <item row="12" column="1">
                <widget class="QLabel" name="label_realsense">
                 <property name="text">
                  <string/>
@@ -818,7 +818,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="30" column="2">
+              <item row="31" column="2">
                <widget class="QLabel" name="label_dvo_license">
                 <property name="text">
                  <string>GPLv3</string>
@@ -828,7 +828,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="33" column="0">
+              <item row="34" column="0">
                <widget class="QLabel" name="label_36">
                 <property name="text">
                  <string>With loam_velodyne :</string>
@@ -861,7 +861,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="26" column="1">
+              <item row="27" column="1">
                <widget class="QLabel" name="label_openchisel">
                 <property name="text">
                  <string/>
@@ -874,7 +874,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="20" column="2">
+              <item row="21" column="2">
                <widget class="QLabel" name="label_gtsam_license">
                 <property name="text">
                  <string>BSD</string>
@@ -884,7 +884,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="23" column="1">
+              <item row="24" column="1">
                <widget class="QLabel" name="label_libpointmatcher">
                 <property name="text">
                  <string/>
@@ -897,7 +897,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="33" column="1">
+              <item row="34" column="1">
                <widget class="QLabel" name="label_loam">
                 <property name="text">
                  <string/>
@@ -910,7 +910,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="9" column="0">
+              <item row="10" column="0">
                <widget class="QLabel" name="label_13">
                 <property name="text">
                  <string>With OpenNI2 :</string>
@@ -920,7 +920,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="30" column="0">
+              <item row="31" column="0">
                <widget class="QLabel" name="label_27">
                 <property name="text">
                  <string>With DVO :</string>
@@ -930,7 +930,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="29" column="1">
+              <item row="30" column="1">
                <widget class="QLabel" name="label_viso2">
                 <property name="text">
                  <string/>
@@ -943,7 +943,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="31" column="2">
+              <item row="32" column="2">
                <widget class="QLabel" name="label_orbslam2_license">
                 <property name="text">
                  <string>GPLv3</string>
@@ -953,7 +953,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="26" column="0">
+              <item row="27" column="0">
                <widget class="QLabel" name="label_30">
                 <property name="text">
                  <string>With OpenChisel :</string>
@@ -976,7 +976,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="18" column="1">
+              <item row="19" column="1">
                <widget class="QLabel" name="label_toro">
                 <property name="text">
                  <string/>
@@ -989,7 +989,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="19" column="2">
+              <item row="20" column="2">
                <widget class="QLabel" name="label_g2o_license">
                 <property name="text">
                  <string>BSD</string>
@@ -999,7 +999,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="32" column="2">
+              <item row="33" column="2">
                <widget class="QLabel" name="label_okvis_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1009,7 +1009,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="33" column="2">
+              <item row="34" column="2">
                <widget class="QLabel" name="label_loam_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1039,7 +1039,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="23" column="2">
+              <item row="24" column="2">
                <widget class="QLabel" name="label_libpointmatcher_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1049,7 +1049,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="25" column="2">
+              <item row="26" column="2">
                <widget class="QLabel" name="label_cputsdf_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1069,7 +1069,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="21" column="2">
+              <item row="22" column="2">
                <widget class="QLabel" name="label_cvsba_license">
                 <property name="text">
                  <string>GPLv2</string>
@@ -1079,7 +1079,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="18" column="2">
+              <item row="19" column="2">
                <widget class="QLabel" name="label_toro_license">
                 <property name="text">
                  <string>Creative Commons [Attribution-NonCommercial-ShareAlike]</string>
@@ -1089,7 +1089,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="11" column="2">
+              <item row="12" column="2">
                <widget class="QLabel" name="label_realsense_license">
                 <property name="text">
                  <string>Apache-2</string>
@@ -1112,7 +1112,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="17" column="0">
+              <item row="18" column="0">
                <widget class="QLabel" name="label_68">
                 <property name="text">
                  <string>With K4A :</string>
@@ -1122,7 +1122,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="16" column="1">
+              <item row="17" column="1">
                <widget class="QLabel" name="label_k4w2">
                 <property name="text">
                  <string/>
@@ -1135,7 +1135,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="17" column="1">
+              <item row="18" column="1">
                <widget class="QLabel" name="label_k4a">
                 <property name="text">
                  <string/>
@@ -1148,7 +1148,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="17" column="2">
+              <item row="18" column="2">
                <widget class="QLabel" name="label_g2o_license_3">
                 <property name="text">
                  <string>MIT</string>
@@ -1158,7 +1158,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="7" column="0">
+              <item row="8" column="0">
                <widget class="QLabel" name="label_39">
                 <property name="text">
                  <string>With FastCV :</string>
@@ -1168,7 +1168,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="7" column="1">
+              <item row="8" column="1">
                <widget class="QLabel" name="label_fastcv">
                 <property name="text">
                  <string/>
@@ -1181,7 +1181,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="7" column="2">
+              <item row="8" column="2">
                <widget class="QLabel" name="label_fastcv_license">
                 <property name="text">
                  <string>Apache v2</string>
@@ -1191,7 +1191,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="22" column="0">
+              <item row="23" column="0">
                <widget class="QLabel" name="label_40">
                 <property name="text">
                  <string>With Ceres :</string>
@@ -1201,7 +1201,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="22" column="2">
+              <item row="23" column="2">
                <widget class="QLabel" name="label_ceres_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1211,7 +1211,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="22" column="1">
+              <item row="23" column="1">
                <widget class="QLabel" name="label_ceres">
                 <property name="text">
                  <string/>
@@ -1224,7 +1224,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="27" column="0">
+              <item row="28" column="0">
                <widget class="QLabel" name="label_41">
                 <property name="text">
                  <string>With AliceVision :</string>
@@ -1234,7 +1234,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="27" column="1">
+              <item row="28" column="1">
                <widget class="QLabel" name="label_aliceVision">
                 <property name="text">
                  <string/>
@@ -1247,7 +1247,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="27" column="2">
+              <item row="28" column="2">
                <widget class="QLabel" name="label_aliceVision_license">
                 <property name="text">
                  <string>MPL2</string>
@@ -1290,6 +1290,39 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
+              <item row="7" column="2">
+               <widget class="QLabel" name="label_pymatcher_license">
+                <property name="text">
+                 <string>PSF</string>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="7" column="0">
+               <widget class="QLabel" name="label_43">
+                <property name="text">
+                 <string>With Python3 :</string>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="7" column="1">
+               <widget class="QLabel" name="label_pymatcher">
+                <property name="text">
+                 <string/>
+                </property>
+                <property name="alignment">
+                 <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
              </layout>
             </item>
            </layout>

guilib/src/ui/mainWindow.ui

@@ -168,6 +168,16 @@
        <addaction name="actionFreenect2"/>
        <addaction name="actionKinect_for_Windows_SDK_v2"/>
       </widget>
+      <widget class="QMenu" name="menuKinect_K4A">
+       <property name="title">
+        <string>Kinect for Azure</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/k4a.png</normaloff>:/images/k4a.png</iconset>
+       </property>
+       <addaction name="actionKinect_for_Azure"/>
+      </widget>
       <widget class="QMenu" name="menuRealSense_R200">
        <property name="title">
         <string>RealSense R200</string>
@@ -222,6 +232,7 @@
       <addaction name="menuXtion_PRO_LIVE"/>
       <addaction name="menuSense_3D_scanner"/>
       <addaction name="menuKinect_v2"/>
+      <addaction name="menuKinect_K4A"/>
       <addaction name="menuRealSense_R200"/>
       <addaction name="menuRealSense_ZR300"/>
       <addaction name="menuRealSense_SR300"/>
@@ -1542,6 +1553,14 @@
     <string>Kinect for Windows SDK v2</string>
    </property>
   </action>
+  <action name="actionKinect_for_Azure">
+   <property name="checkable">
+    <bool>false</bool>
+   </property>
+   <property name="text">
+    <string>Kinect for Azure</string>
+   </property>
+  </action>
   <action name="actionRealSense2_D435">
    <property name="checkable">
     <bool>true</bool>
@@ -1591,6 +1610,9 @@
    </property>
   </action>
   <action name="actionMYNT_EYE_S_SDK">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
    <property name="text">
     <string>MYNT EYE S SDK</string>
    </property>

guilib/src/ui/postProcessingDialog.ui

@@ -337,7 +337,7 @@
    <item>
     <widget class="QLabel" name="label_4">
      <property name="text">
-      <string>Note: Post-processing only affects visualization in the GUI and the export actions. To actually push those optimizations to the database, use Database Viewer tool (Tools-&gt;Edit database...).</string>
+      <string>Note: Post-processing only affects visualization in the GUI and the export actions. To actually push those optimizations to the database, use Database Viewer tool (Tools-&gt;Edit database...). You can do Edit-&gt;Download graph to revert the changes afterwards.</string>
      </property>
      <property name="wordWrap">
       <bool>true</bool>

guilib/src/utilite/UPlot.cpp

@@ -36,6 +36,7 @@
 #include <QToolButton>
 #include <QLabel>
 #include <QMenu>
+#include <QStyle>
 #include <QInputDialog>
 #include <QMessageBox>
 #include <QFileDialog>
@@ -48,6 +49,7 @@
 #include <QtSvg/QSvgGenerator>
 #endif
 #include <cmath>
+#include <limits>
 
 #define PRINT_DEBUG 0
 
@@ -1452,18 +1454,7 @@ void UPlotLegendItem::contextMenuEvent(QContextMenuEvent * event)
 	}
 	else if(action == _aShowStdDevMeanMax)
 	{
-		if(_aShowStdDevMeanMax->isChecked())
-		{
-			connect(_curve, SIGNAL(dataChanged(const UPlotCurve *)), this, SLOT(updateStdDevMeanMax()));
-			updateStdDevMeanMax();
-		}
-		else
-		{
-			disconnect(_curve, SIGNAL(dataChanged(const UPlotCurve *)), this, SLOT(updateStdDevMeanMax()));
-			QString nameSpaced = _curve->name();
-			nameSpaced.replace('_', ' ');
-			this->setText(nameSpaced);
-		}
+		showStdDevMeanMax(_aShowStdDevMeanMax->isChecked());
 	}
 	else if(action == _aRemoveCurve)
 	{
@@ -1479,6 +1470,23 @@ void UPlotLegendItem::contextMenuEvent(QContextMenuEvent * event)
 	}
 }
 
+void UPlotLegendItem::showStdDevMeanMax(bool shown)
+{
+	_aShowStdDevMeanMax->setChecked(shown);
+	if(shown)
+	{
+		connect(_curve, SIGNAL(dataChanged(const UPlotCurve *)), this, SLOT(updateStdDevMeanMax()));
+		updateStdDevMeanMax();
+	}
+	else
+	{
+		disconnect(_curve, SIGNAL(dataChanged(const UPlotCurve *)), this, SLOT(updateStdDevMeanMax()));
+		QString nameSpaced = _curve->name();
+		nameSpaced.replace('_', ' ');
+		this->setText(nameSpaced);
+	}
+}
+
 QPixmap UPlotLegendItem::createSymbol(const QPen & pen, const QBrush & brush)
 {
 	QPixmap pixmap(50, 50);
@@ -1500,7 +1508,7 @@ void UPlotLegendItem::updateStdDevMeanMax()
 	float max = uMax(y.data(), y.size());
 	QString nameSpaced = _curve->name();
 	nameSpaced.replace('_', ' ');
-	nameSpaced += QString("\n(%1=%2, %3=%4, max=%5)").arg(QChar(0xbc, 0x03)).arg(mean).arg(QChar(0xc3, 0x03)).arg(stdDev).arg(max);
+	nameSpaced += QString("\n(%1=%2, %3=%4, max=%5, n=%6)").arg(QChar(0xbc, 0x03)).arg(QString::number(mean, 'f', 3)).arg(QChar(0xc3, 0x03)).arg(QString::number(stdDev, 'f', 3)).arg(QString::number(max, 'f', 3)).arg(y.size());
 	this->setText(nameSpaced);
 }
 
@@ -1518,15 +1526,30 @@ UPlotLegend::UPlotLegend(QWidget * parent) :
 	_aUseFlatButtons->setCheckable(true);
 	_aUseFlatButtons->setChecked(_flat);
 	_aCopyAllCurvesToClipboard = new QAction(tr("Copy all curve data to clipboard"), this);
+	_aShowAllStdDevMeanMax = new QAction(tr("Show all %1, %2, max").arg(QChar(0xbc, 0x03)).arg(QChar(0xc3, 0x03)), this);
+	_aShowAllStdDevMeanMax->setCheckable(true);
+	_aShowAllStdDevMeanMax->setChecked(false);
 	_menu = new QMenu(tr("Legend"), this);
 	_menu->addAction(_aUseFlatButtons);
+	_menu->addAction(_aShowAllStdDevMeanMax);
 	_menu->addAction(_aCopyAllCurvesToClipboard);
 
-	QVBoxLayout * vLayout = new QVBoxLayout(this);
-	vLayout->setContentsMargins(0,0,0,0);
-	this->setLayout(vLayout);
-	vLayout->addStretch(0);
-	vLayout->setSpacing(0);
+	_scrollArea = new QScrollArea(this);
+	_scrollArea->setWidgetResizable( true );
+	_scrollArea->setFrameShape(QFrame::NoFrame);
+
+	this->setLayout(new QVBoxLayout());
+	this->layout()->setContentsMargins(0,0,0,0);
+	this->layout()->addWidget(_scrollArea);
+
+	QWidget * _scrollAreaWidgetContent = new QWidget();
+	_scrollArea->setWidget( _scrollAreaWidgetContent );
+
+	_contentLayout = new QVBoxLayout();
+	_scrollAreaWidgetContent->setLayout(_contentLayout);
+	_contentLayout->setContentsMargins(0,0,0,0);
+	((QVBoxLayout*)_contentLayout)->addStretch(0);
+	_contentLayout->setSpacing(0);
 }
 
 UPlotLegend::~UPlotLegend()
@@ -1572,7 +1595,9 @@ void UPlotLegend::addItem(UPlotCurve * curve)
 		hLayout->setMargin(0);
 
 		// add to the legend
-		((QVBoxLayout*)this->layout())->insertLayout(this->layout()->count()-1, hLayout);
+		((QVBoxLayout*)_contentLayout)->insertLayout(_contentLayout->count()-1, hLayout);
+
+		_scrollArea->setMinimumWidth(std::min(480, _scrollArea->widget()->sizeHint().width()+QApplication::style()->pixelMetric(QStyle::PM_ScrollBarExtent)));
 	}
 }
 
@@ -1584,6 +1609,7 @@ bool UPlotLegend::remove(const UPlotCurve * curve)
 		if(items.at(i)->curve() == curve)
 		{
 			delete items.at(i);
+			_scrollArea->setMinimumWidth(std::min(480, _scrollArea->widget()->sizeHint().width()+QApplication::style()->pixelMetric(QStyle::PM_ScrollBarExtent)));
 			return true;
 		}
 	}
@@ -1602,24 +1628,24 @@ void UPlotLegend::moveUp(UPlotLegendItem * item)
 {
 	int index = -1;
 	QLayoutItem * layoutItem = 0;
-	for(int i=0; i<this->layout()->count(); ++i)
+	for(int i=0; i<_contentLayout->count(); ++i)
 	{
-		if(this->layout()->itemAt(i)->layout() &&
-		   this->layout()->itemAt(i)->layout()->indexOf(item) != -1)
+		if(_contentLayout->itemAt(i)->layout() &&
+				_contentLayout->itemAt(i)->layout()->indexOf(item) != -1)
 		{
-			layoutItem = this->layout()->itemAt(i);
+			layoutItem = _contentLayout->itemAt(i);
 			index = i;
 			break;
 		}
 	}
 	if(index > 0 && layoutItem)
 	{
-		this->layout()->removeItem(layoutItem);
+		_contentLayout->removeItem(layoutItem);
 		QHBoxLayout * hLayout = new QHBoxLayout();
 		hLayout->addWidget(layoutItem->layout()->itemAt(0)->widget());
 		hLayout->addStretch(0);
 		hLayout->setMargin(0);
-		((QVBoxLayout*)this->layout())->insertLayout(index-1, hLayout);
+		((QVBoxLayout*)_contentLayout)->insertLayout(index-1, hLayout);
 		delete layoutItem;
 		Q_EMIT legendItemMoved(item->curve(), index-1);
 	}
@@ -1629,24 +1655,24 @@ void UPlotLegend::moveDown(UPlotLegendItem * item)
 {
 	int index = -1;
 	QLayoutItem * layoutItem = 0;
-	for(int i=0; i<this->layout()->count(); ++i)
+	for(int i=0; i<_contentLayout->count(); ++i)
 	{
-		if(this->layout()->itemAt(i)->layout() &&
-		   this->layout()->itemAt(i)->layout()->indexOf(item) != -1)
+		if(_contentLayout->itemAt(i)->layout() &&
+		   _contentLayout->itemAt(i)->layout()->indexOf(item) != -1)
 		{
-			layoutItem = this->layout()->itemAt(i);
+			layoutItem = _contentLayout->itemAt(i);
 			index = i;
 			break;
 		}
 	}
-	if(index < this->layout()->count()-2 && layoutItem)
+	if(index < _contentLayout->count()-2 && layoutItem)
 	{
-		this->layout()->removeItem(layoutItem);
+		_contentLayout->removeItem(layoutItem);
 		QHBoxLayout * hLayout = new QHBoxLayout();
 		hLayout->addWidget(layoutItem->layout()->itemAt(0)->widget());
 		hLayout->addStretch(0);
 		hLayout->setMargin(0);
-		((QVBoxLayout*)this->layout())->insertLayout(index+1, hLayout);
+		((QVBoxLayout*)_contentLayout)->insertLayout(index+1, hLayout);
 		delete layoutItem;
 		Q_EMIT legendItemMoved(item->curve(), index+1);
 	}
@@ -1738,6 +1764,14 @@ void UPlotLegend::contextMenuEvent(QContextMenuEvent * event)
 			}
 		}
 	}
+	else if(action == _aShowAllStdDevMeanMax)
+	{
+		QList<UPlotLegendItem *> items = this->findChildren<UPlotLegendItem*>();
+		for(int i=0; i<items.size(); ++i)
+		{
+			items.at(i)->showStdDevMeanMax(_aShowAllStdDevMeanMax->isChecked());
+		}
+	}
 }
 
 void UPlotLegend::redirectToggled(bool toggled)

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.19.4</version>
+  <version>0.20.0</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -18,7 +18,6 @@
   <build_depend>zlib</build_depend>
   <build_depend>libfreenect-dev</build_depend>
   <build_depend>libopenni-dev</build_depend>
-  <build_depend>libvtk-qt</build_depend>
   <!--<build_depend>libopenni2-dev</build_depend> --> <!-- not available on Jessie -->
   <build_depend>cv_bridge</build_depend>
   <!-- libproj-dev needed due to error in vtk6 (kinetic)-->
@@ -32,7 +31,6 @@
   <run_depend>zlib</run_depend>
   <run_depend>libfreenect-dev</run_depend>
   <run_depend>libopenni-dev</run_depend>
-  <run_depend>libvtk-qt</run_depend>
   <!-- <run_depend>libopenni2-dev</run_depend> -->
   <run_depend>cv_bridge</run_depend>
   <run_depend>octomap</run_depend>

tools/CMakeLists.txt

@@ -25,6 +25,7 @@ IF(TARGET rtabmap_gui)
 	ADD_SUBDIRECTORY( OdometryViewer )
 	ADD_SUBDIRECTORY( DataRecorder )
 	ADD_SUBDIRECTORY( Calibration )
+	ADD_SUBDIRECTORY( Matcher )
 ELSE()
    MESSAGE(STATUS "RTAB-Map GUI lib is not built, some tools won't be built...")
 ENDIF()

tools/CameraRGBD/main.cpp

@@ -303,7 +303,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with Kinect for Azure SDK support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraK4A(deviceId);
+		camera = new rtabmap::CameraK4A(1, 0, rtabmap::Transform::getIdentity());
 	}
 	else if (driver == 13)
 	{

tools/EpipolarGeometry/main.cpp

@@ -175,8 +175,8 @@ int main(int argc, char** argv)
     cv::Mat image2;
 	if(argc == 3)
 	{
-		image1 = cv::imread(argv[1]);
-		image2 = cv::imread(argv[2]);
+		image1 = cv::imread(argv[1], cv::IMREAD_GRAYSCALE);
+		image2 = cv::imread(argv[2], cv::IMREAD_GRAYSCALE);
 	}
 	else
 	{

tools/Export/main.cpp

@@ -282,7 +282,7 @@ int main(int argc, char * argv[])
 	std::map<int, Transform> optimizedPoses;
 	std::multimap<int, Link> links;
 	printf("Optimizing the map...\n");
-	rtabmap.get3DMap(nodes, optimizedPoses, links, true, true);
+	rtabmap.getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true);
 	printf("Optimizing the map... done (%fs).\n", timer.ticks());
 
 	std::string outputDirectory = UDirectory::getDir(dbPath);

tools/Info/main.cpp

@@ -94,6 +94,11 @@ int main(int argc, char * argv[])
 	}
 
 	std::string databasePath = uReplaceChar(argv[argc-1], '~', UDirectory::homeDir());
+	if(!UFile::exists(databasePath))
+	{
+		printf("Database \"%s\" doesn't exist!\n", databasePath.c_str());
+		return -1;
+	}
 
 	DBDriver * driver = DBDriver::create();
 	if(!driver->openConnection(databasePath))
@@ -106,11 +111,125 @@ int main(int argc, char * argv[])
 	ParametersMap parameters = driver->getLastParameters();
 	ParametersMap defaultParameters = Parameters::getDefaultParameters();
 	ParametersMap removedParameters = Parameters::getBackwardCompatibilityMap();
+	std::string otherDatabasePathName;
+	if(!otherDatabasePath.empty())
+	{
+		driver->closeConnection(false);
+
+		if(!UFile::exists(otherDatabasePath))
+		{
+			printf("Database \"%s\" doesn't exist!\n", otherDatabasePath.c_str());
+			delete driver;
+			return -1;
+		}
+
+		if(!driver->openConnection(otherDatabasePath))
+		{
+			printf("Cannot open database \"%s\".\n", otherDatabasePath.c_str());
+			delete driver;
+			return -1;
+		}
+		otherDatabasePathName = UFile::getName(otherDatabasePath);
+		defaultParameters = driver->getLastParameters();
+		removedParameters.clear();
+	}
+
+#ifdef _WIN32
+	HANDLE H = GetStdHandle(STD_OUTPUT_HANDLE);
+#endif
+	int padding = 35;
+	std::cout << ("Parameters (Yellow=modified, Red=old parameter not used anymore):\n");
+	for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		ParametersMap::const_iterator jter = defaultParameters.find(iter->first);
+		std::string defaultValue;
+		bool defaultValueSet = false;
+		if(jter == defaultParameters.end())
+		{
+			jter = removedParameters.find(iter->first);
+			if(jter != removedParameters.end())
+			{
+				defaultValue = jter->second;
+				defaultValueSet = true;
+			}
+		}
+		else
+		{
+			defaultValue = jter->second;
+			defaultValueSet = true;
+		}
+
+		if(defaultValueSet &&
+		   iter->second.compare(defaultValue) != 0 &&
+		   iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
+		{
+			bool different = true;
+			if(Parameters::getType(iter->first).compare("double") ==0 ||
+			   Parameters::getType(iter->first).compare("float") == 0)
+			{
+				if(uStr2Double(iter->second) == uStr2Double(defaultValue))
+				{
+					different = false;
+				}
+			}
+
+			if(different)
+			{
+				//yellow
+#ifdef _WIN32
+				SetConsoleTextAttribute(H,COLOR_YELLOW);
+#else
+				printf("%s", COLOR_YELLOW);
+#endif
+				std::cout << (uFormat("%s%s  (%s=%s)\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str(), otherDatabasePath.empty()?"default":otherDatabasePathName.c_str(), defaultValue.c_str()));
+			}
+			else if(!diff)
+			{
+				//green
+#ifdef _WIN32
+				SetConsoleTextAttribute(H,COLOR_NORMAL);
+#else
+				printf("%s", COLOR_NORMAL);
+#endif
+				std::cout << (uFormat("%s%s\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str()));
+			}
+		}
+		else if(!defaultValueSet && otherDatabasePath.empty())
+		{
+			//red
+#ifdef _WIN32
+			SetConsoleTextAttribute(H,COLOR_RED);
+#else
+			printf("%s", COLOR_RED);
+#endif
+			std::cout << (uFormat("%s%s\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str()));
+		}
+		else if(!diff)
+		{
+			//green
+#ifdef _WIN32
+			SetConsoleTextAttribute(H,COLOR_NORMAL);
+#else
+			printf("%s", COLOR_NORMAL);
+#endif
+			std::cout << (uFormat("%s%s\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str()));
+		}
+#ifdef _WIN32
+		SetConsoleTextAttribute(H,COLOR_NORMAL);
+#else
+		printf("%s", COLOR_NORMAL);
+#endif
+	}
+
 	if(otherDatabasePath.empty())
 	{
+		printf("\nInfo:\n\n");
 		std::string info;
 		std::set<int> ids;
 		driver->getAllNodeIds(ids);
+		Transform lastLocalization;
+		std::map<int, Transform> optimizedPoses = driver->loadOptimizedPoses(&lastLocalization);
+		std::set<int> mapsLinkedToLastGraph;
 		int lastMapId=0;
 		double previousStamp = 0.0f;
 		Transform previousPose;
@@ -144,6 +263,10 @@ int main(int argc, char * argv[])
 			{
 				++gpsValues;
 			}
+			if(optimizedPoses.find(id) != optimizedPoses.end())
+			{
+				mapsLinkedToLastGraph.insert(mapId);
+			}
 			if(iter!=ids.begin())
 			{
 				if(lastMapId == mapId)
@@ -193,10 +316,23 @@ int main(int argc, char * argv[])
 		{
 			std::cout << (uFormat("%s%f m\n", pad("Total odometry length:").c_str(), infoTotalOdom));
 		}
+
+		std::stringstream sessionsInOptGraphStr;
+		for(std::set<int>::iterator iter=mapsLinkedToLastGraph.begin(); iter!=mapsLinkedToLastGraph.end(); ++iter)
+		{
+			if(iter!=mapsLinkedToLastGraph.begin())
+			{
+				sessionsInOptGraphStr << ", ";
+			}
+			sessionsInOptGraphStr << *iter;
+		}
+
 		std::cout << (uFormat("%s%fs\n", pad("Total time:").c_str(), infoTotalTime));
 		std::cout << (uFormat("%s%d nodes and %d words\n", pad("LTM:").c_str(), (int)ids.size(), driver->getTotalDictionarySize()));
 		std::cout << (uFormat("%s%d nodes and %d words\n", pad("WM:").c_str(), driver->getLastNodesSize(), driver->getLastDictionarySize()));
 		std::cout << (uFormat("%s%d poses and %d links\n", pad("Global graph:").c_str(), odomPoses, links.size()));
+		std::cout << (uFormat("%s%d poses\n", pad("Optimized graph:").c_str(), (int)optimizedPoses.size(), links.size()));
+		std::cout << (uFormat("%s%d/%d [%s]\n", pad("Maps in graph:").c_str(), (int)mapsLinkedToLastGraph.size(), sessions, sessionsInOptGraphStr.str().c_str()));
 		std::cout << (uFormat("%s%d poses\n", pad("Ground truth:").c_str(), gtPoses));
 		std::cout << (uFormat("%s%d poses\n", pad("GPS:").c_str(), gpsValues));
 		std::cout << (uFormat("Links:\n"));
@@ -245,105 +381,6 @@ int main(int argc, char * argv[])
 		mem = dbSize - total;
 		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Other (indexing):").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		std::cout << ("\n");
-		std::cout << ("Parameters (Yellow=modified, Red=old parameter not used anymore):\n");
-	}
-	else
-	{
-		driver->closeConnection(false);
-		if(!driver->openConnection(otherDatabasePath))
-		{
-			printf("Cannot open database \"%s\".\n", otherDatabasePath.c_str());
-			delete driver;
-			return -1;
-		}
-		defaultParameters = driver->getLastParameters();
-		removedParameters.clear();
-	}
-
-#ifdef _WIN32
-	HANDLE H = GetStdHandle(STD_OUTPUT_HANDLE);
-#endif
-	int padding = 35;
-	for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
-	{
-		ParametersMap::const_iterator jter = defaultParameters.find(iter->first);
-		std::string defaultValue;
-		bool defaultValueSet = false;
-		if(jter == defaultParameters.end())
-		{
-			jter = removedParameters.find(iter->first);
-			if(jter != removedParameters.end())
-			{
-				defaultValue = jter->second;
-				defaultValueSet = true;
-			}
-		}
-		else
-		{
-			defaultValue = jter->second;
-			defaultValueSet = true;
-		}
-
-		if(defaultValueSet &&
-		   iter->second.compare(defaultValue) != 0 &&
-		   iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
-		{
-			bool different = true;
-			if(Parameters::getType(iter->first).compare("double") ==0 ||
-			   Parameters::getType(iter->first).compare("float") == 0)
-			{
-				if(uStr2Double(iter->second) == uStr2Double(defaultValue))
-				{
-					different = false;
-				}
-			}
-
-			if(different)
-			{
-				//yellow
-#ifdef _WIN32
-				SetConsoleTextAttribute(H,COLOR_YELLOW);
-#else
-				printf("%s", COLOR_YELLOW);
-#endif
-				std::cout << (uFormat("%s%s  (%s=%s)\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str(), otherDatabasePath.empty()?"default":"other", defaultValue.c_str()));
-			}
-			else if(!diff)
-			{
-				//green
-#ifdef _WIN32
-				SetConsoleTextAttribute(H,COLOR_NORMAL);
-#else
-				printf("%s", COLOR_NORMAL);
-#endif
-				std::cout << (uFormat("%s%s\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str()));
-			}
-		}
-		else if(!defaultValueSet && otherDatabasePath.empty())
-		{
-			//red
-#ifdef _WIN32
-			SetConsoleTextAttribute(H,COLOR_RED);
-#else
-			printf("%s", COLOR_RED);
-#endif
-			std::cout << (uFormat("%s%s\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str()));
-		}
-		else if(!diff)
-		{
-			//green
-#ifdef _WIN32
-			SetConsoleTextAttribute(H,COLOR_NORMAL);
-#else
-			printf("%s", COLOR_NORMAL);
-#endif
-			std::cout << (uFormat("%s%s\n", pad(iter->first + "=", padding).c_str(), iter->second.c_str()));
-		}
-#ifdef _WIN32
-		SetConsoleTextAttribute(H,COLOR_NORMAL);
-#else
-		printf("%s", COLOR_NORMAL);
-#endif
 	}
 
 	return 0;

tools/Matcher/CMakeLists.txt

@@ -0,0 +1,28 @@
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/guilib/include
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+IF(QT4_FOUND)
+    INCLUDE(${QT_USE_FILE})
+ENDIF(QT4_FOUND)
+
+SET(LIBRARIES
+	${OpenCV_LIBRARIES} 
+	${PCL_LIBRARIES}
+	${QT_LIBRARIES} 
+)
+
+add_definitions(${PCL_DEFINITIONS})
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(matcher main.cpp)
+TARGET_LINK_LIBRARIES(matcher rtabmap_core rtabmap_utilite rtabmap_gui ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( matcher 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-matcher)
+ 

tools/Matcher/main.cpp

@@ -0,0 +1,541 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/RegistrationVis.h>
+#include <rtabmap/core/EpipolarGeometry.h>
+#include <rtabmap/core/Features2d.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/VWDictionary.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_features.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/gui/ImageView.h>
+#include <rtabmap/gui/KeypointItem.h>
+#include <rtabmap/gui/CloudViewer.h>
+#include <rtabmap/utilite/UCv2Qt.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <fstream>
+#include <string>
+#include <QApplication>
+#include <QDialog>
+#include <QHBoxLayout>
+#include <QMultiMap>
+#include <QString>
+#include <opencv2/core/core.hpp>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\n\nUsage:\n"
+			"   rtabmap-matcher [Options] from.png to.png\n"
+			"Examples:\n"
+			"   rtabmap-matcher --Vis/CorNNType 5 --Vis/PnPReprojError 3 from.png to.png\n"
+			"   rtabmap-matcher --Vis/CorNNDR 0.8 from.png to.png\n"
+			"   rtabmap-matcher --Vis/FeatureType 11 --SuperPoint/ModelPath \"superpoint.pt\" --Vis/CorNNType 6 --PyMatcher/Path \"~/SuperGluePretrainedNetwork/rtabmap_superglue.py\" from.png to.png\n"
+			"   rtabmap-matcher --Vis/FeatureType 1 --Vis/CorNNType 6 --PyMatcher/Path \"~/OANet/demo/rtabmap_oanet.py\" --PyMatcher/Model \"~/OANet/model/gl3d/sift-4000/model_best.pth\" from.png to.png\n"
+			"   rtabmap-matcher --calibration calib.yaml --from_depth from_depth.png --to_depth to_depth.png from.png to.png\n"
+			"   rtabmap-matcher --calibration calib.yaml --Vis/FeatureType 2 --Vis/MaxFeatures 10000 --Vis/CorNNType 7 from.png to.png\n"
+			"\n"
+			"Note: Use \"Vis/\" parameters for feature stuff.\n"
+			"Options:\n"
+			"   --calibration \"calibration.yaml\" Calibration file. If not set, a\n"
+			"                                        fake one is created from image's\n"
+			"                                        size (which may not be optimal).\n"
+			"                                        Required if from_depth option is set.\n"
+			"                                        Assuming same calibration for both images.\n"
+			"   --from_depth \"from_depth.png\"    Depth or right image file of the first image.\n"
+			"                                        If not set, 2D->2D estimation is done by \n"
+			"                                        default. For 3D->2D estimation, from_depth\n"
+			"                                        should be set.\n"
+			"   --to_depth \"to_depth.png\"        Depth or right image file of the second image.\n"
+			"                                        For 3D->3D estimation, from_depth and to_depth\n"
+			"                                        should be both set.\n"
+			"\n\n"
+			"%s\n",
+			Parameters::showUsage());
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	if(argc < 3)
+	{
+		showUsage();
+	}
+
+	ULogger::setLevel(ULogger::kWarning);
+	ULogger::setType(ULogger::kTypeConsole);
+
+	std::string fromDepthPath;
+	std::string toDepthPath;
+	std::string calibrationPath;
+	for(int i=1; i<argc-2; ++i)
+	{
+		if(strcmp(argv[i], "--from_depth") == 0)
+		{
+			++i;
+			if(i<argc-2)
+			{
+				fromDepthPath = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(strcmp(argv[i], "--to_depth") == 0)
+		{
+			++i;
+			if(i<argc-2)
+			{
+				toDepthPath = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(strcmp(argv[i], "--calibration") == 0)
+		{
+			++i;
+			if(i<argc-2)
+			{
+				calibrationPath = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(strcmp(argv[i], "--help") == 0)
+		{
+			showUsage();
+		}
+	}
+
+	printf("Options\n");
+	printf("  --calibration = \"%s\"\n", calibrationPath.c_str());
+	printf("  --from_depth  = \"%s\"\n", fromDepthPath.c_str());
+	printf("  --to_depth    = \"%s\"\n", toDepthPath.c_str());
+
+	ParametersMap parameters = Parameters::parseArguments(argc, argv);
+	parameters.insert(ParametersPair(Parameters::kRegRepeatOnce(), "false"));
+
+	cv::Mat imageFrom = cv::imread(argv[argc-2], cv::IMREAD_COLOR);
+	cv::Mat imageTo = cv::imread(argv[argc-1], cv::IMREAD_COLOR);
+
+	if(!imageFrom.empty() && !imageTo.empty())
+	{
+		//////////////////
+		// Load data
+		//////////////////
+
+		cv::Mat fromDepth;
+		cv::Mat toDepth;
+		if(!calibrationPath.empty())
+		{
+			if(!fromDepthPath.empty())
+			{
+				fromDepth = cv::imread(fromDepthPath, cv::IMREAD_UNCHANGED);
+				if(fromDepth.type() == CV_8UC3)
+				{
+					cv::cvtColor(fromDepth, fromDepth, cv::COLOR_BGR2GRAY);
+				}
+				else if(fromDepth.empty())
+				{
+					printf("Failed loading from_depth image: \"%s\"!", fromDepthPath.c_str());
+				}
+			}
+			if(!toDepthPath.empty())
+			{
+				toDepth = cv::imread(toDepthPath, cv::IMREAD_UNCHANGED);
+				if(toDepth.type() == CV_8UC3)
+				{
+					cv::cvtColor(toDepth, toDepth, cv::COLOR_BGR2GRAY);
+				}
+				else if(toDepth.empty())
+				{
+					printf("Failed loading to_depth image: \"%s\"!", toDepthPath.c_str());
+				}
+			}
+			UASSERT(toDepth.empty() || (!fromDepth.empty() && fromDepth.type() == toDepth.type()));
+		}
+		else if(!fromDepthPath.empty() || !fromDepthPath.empty())
+		{
+			printf("A calibration file should be provided if depth images are used!\n");
+			showUsage();
+		}
+
+		CameraModel model;
+		StereoCameraModel stereoModel;
+		if(!fromDepth.empty())
+		{
+			if(fromDepth.type() != CV_8UC1)
+			{
+				if(!model.load(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()))
+				{
+					printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
+					exit(-1);
+				}
+			}
+			else // fromDepth.type() == CV_8UC1
+			{
+				if(!stereoModel.load(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()))
+				{
+					printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
+					exit(-1);
+				}
+			}
+		}
+		else if(!calibrationPath.empty())
+		{
+			if(!model.load(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()))
+			{
+				printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
+				exit(-1);
+			}
+		}
+		else
+		{
+			printf("Using fake calibration model (image size=%dx%d): fx=%d fy=%d cx=%d cy=%d\n",
+					imageFrom.cols, imageFrom.rows, imageFrom.cols/2, imageFrom.cols/2, imageFrom.cols/2, imageFrom.rows/2);
+			model = CameraModel(imageFrom.cols/2, imageFrom.cols/2, imageFrom.cols/2, imageFrom.rows/2); // Fake model
+			model.setImageSize(imageFrom.size());
+		}
+
+		Signature dataFrom;
+		Signature dataTo;
+		if(model.isValidForProjection())
+		{
+			printf("Mono calibration model detected.\n");
+			dataFrom = SensorData(imageFrom, fromDepth, model, 1);
+			dataTo = SensorData(imageTo, toDepth, model, 2);
+		}
+		else //stereo
+		{
+			printf("Stereo calibration model detected.\n");
+			dataFrom = SensorData(imageFrom, fromDepth, stereoModel, 1);
+			dataTo = SensorData(imageTo, toDepth, stereoModel, 2);
+		}
+
+		//////////////////
+		// Registration
+		//////////////////
+
+		if(fromDepth.empty())
+		{
+			parameters.insert(ParametersPair(Parameters::kVisEstimationType(), "2")); // Set 2D->2D estimation for mono images
+			parameters.insert(ParametersPair(Parameters::kVisEpipolarGeometryVar(), "1")); //Unknown scale
+			printf("Calibration not set, setting %s=1 and %s=2 by default (2D->2D estimation)\n", Parameters::kVisEpipolarGeometryVar().c_str(), Parameters::kVisEstimationType().c_str());
+		}
+		RegistrationVis reg(parameters);
+		RegistrationInfo info;
+
+
+		// Do it one time before to make sure everything is loaded to get realistic timing.
+		reg.computeTransformationMod(dataFrom, dataTo, Transform(), &info);
+
+		UTimer timer;
+		Transform t = reg.computeTransformationMod(dataFrom, dataTo, Transform(), &info);
+		double matchingTime = timer.ticks();
+		printf("Time matching and motion estimation: %fs\n", matchingTime);
+
+		//////////////////
+		// Visualization
+		//////////////////
+
+		if(reg.getNNType()==6 &&
+		   !dataFrom.getWordsDescriptors().empty() &&
+		   dataFrom.getWordsDescriptors().begin()->second.type()!=CV_32F)
+		{
+			UWARN("PyMatcher is selected for matching but binary features "
+				  "are not compatible. BruteForce with CrossCheck (%s=5) "
+				  "has been used instead.", Parameters::kVisCorNNType().c_str());
+		}
+
+		QApplication app(argc, argv);
+		QDialog dialog;
+		float reprojError = Parameters::defaultVisPnPReprojError();
+		std::string pyMatcherPath;
+		Parameters::parse(parameters, Parameters::kVisPnPReprojError(), reprojError);
+		Parameters::parse(parameters, Parameters::kPyMatcherPath(), pyMatcherPath);
+		dialog.setWindowTitle(QString("Matches (%1/%2) %3 sec [%4=%5 (%6) %7=%8 (%9)%10 %11=%12 (%13) %14=%15]")
+				.arg(info.inliers)
+				.arg(info.matches)
+				.arg(matchingTime)
+				.arg(Parameters::kVisFeatureType().c_str())
+				.arg(reg.getDetector()?reg.getDetector()->getType():-1)
+				.arg(reg.getDetector()?Feature2D::typeName(reg.getDetector()->getType()).c_str():"?")
+				.arg(Parameters::kVisCorNNType().c_str())
+				.arg(reg.getNNType())
+				.arg(reg.getNNType()<VWDictionary::kNNUndef?VWDictionary::nnStrategyName((VWDictionary::NNStrategy)reg.getNNType()).c_str():
+						reg.getNNType()==5||(reg.getNNType()==6&&!dataFrom.getWordsDescriptors().empty()&& dataFrom.getWordsDescriptors().begin()->second.type()!=CV_32F)?"BFCrossCheck":
+						reg.getNNType()==6?QString(uSplit(UFile::getName(pyMatcherPath), '.').front().c_str()).replace("rtabmap_", ""):
+						reg.getNNType()==7?"GMS":"?")
+				.arg(reg.getNNType()<5?QString(" %1=%2").arg(Parameters::kVisCorNNDR().c_str()).arg(reg.getNNDR()):"")
+				.arg(Parameters::kVisEstimationType().c_str())
+				.arg(reg.getEstimationType())
+				.arg(reg.getEstimationType()==0?"3D->3D":reg.getEstimationType()==1?"3D->2D":reg.getEstimationType()==2?"2D->2D":"?")
+				.arg(Parameters::kVisPnPReprojError().c_str())
+				.arg(reprojError));
+
+		CloudViewer * viewer = 0;
+		if(!t.isNull())
+		{
+			viewer = new CloudViewer(&dialog);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom = util3d::cloudRGBFromSensorData(dataFrom.sensorData());
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo = util3d::cloudRGBFromSensorData(dataTo.sensorData());
+			viewer->addCloud(uFormat("cloud_%d", dataFrom.id()), cloudFrom, Transform::getIdentity(), Qt::magenta);
+			viewer->addCloud(uFormat("cloud_%d", dataTo.id()), cloudTo, t, Qt::cyan);
+			viewer->addOrUpdateCoordinate(uFormat("frame_%d", dataTo.id()), t, 0.2);
+			viewer->setGridShown(true);
+
+			if(reg.getEstimationType() == 2)
+			{
+				// triangulate 3D words based on the transform computed
+				std::map<int, cv::Point3f> points3d = util3d::generateWords3DMono(
+						uMultimapToMapUnique(dataFrom.getWords()),
+						uMultimapToMapUnique(dataTo.getWords()),
+						model.isValidForProjection()?model:stereoModel.left(),
+						t);
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
+				cloudWordsFrom->resize(points3d.size());
+				int i=0;
+				for(std::multimap<int, cv::Point3f>::const_iterator iter=points3d.begin();
+					iter!=points3d.end();
+					++iter)
+				{
+					cloudWordsFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
+				}
+				if(cloudWordsFrom->size())
+				{
+					cloudWordsFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsFrom);
+				}
+				if(cloudWordsFrom->size())
+				{
+					viewer->addCloud("wordsFrom", cloudWordsFrom, Transform::getIdentity(), Qt::yellow);
+					viewer->setCloudPointSize("wordsFrom", 5);
+				}
+			}
+			else
+			{
+				if(!dataFrom.getWords3().empty())
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
+					cloudWordsFrom->resize(dataFrom.getWords3().size());
+					int i=0;
+					for(std::multimap<int, cv::Point3f>::const_iterator iter=dataFrom.getWords3().begin();
+						iter!=dataFrom.getWords3().end();
+						++iter)
+					{
+						cloudWordsFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
+					}
+					if(cloudWordsFrom->size())
+					{
+						cloudWordsFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsFrom);
+					}
+					if(cloudWordsFrom->size())
+					{
+						viewer->addCloud("wordsFrom", cloudWordsFrom, Transform::getIdentity(), Qt::magenta);
+						viewer->setCloudPointSize("wordsFrom", 5);
+					}
+				}
+				if(!dataTo.getWords3().empty())
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
+					cloudWordsFrom->resize(dataTo.getWords3().size());
+					int i=0;
+					for(std::multimap<int, cv::Point3f>::const_iterator iter=dataTo.getWords3().begin();
+						iter!=dataTo.getWords3().end();
+						++iter)
+					{
+						cloudWordsFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
+					}
+					if(cloudWordsFrom->size())
+					{
+						cloudWordsFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsFrom);
+					}
+					if(cloudWordsFrom->size())
+					{
+						viewer->addCloud("wordsTo", cloudWordsFrom, t, Qt::cyan);
+						viewer->setCloudPointSize("wordsTo", 5);
+					}
+				}
+			}
+		}
+
+		QBoxLayout * mainLayout = new QHBoxLayout();
+		mainLayout->setContentsMargins(0, 0, 0, 0);
+		mainLayout->setSpacing(0);
+		QBoxLayout * layout;
+		bool vertical=true;
+		if(imageFrom.cols > imageFrom.rows)
+		{
+			dialog.setMinimumWidth(640*(viewer?2:1));
+			dialog.setMinimumHeight(640*imageFrom.rows/imageFrom.cols*2);
+			layout = new QVBoxLayout();
+		}
+		else
+		{
+			dialog.setMinimumWidth((640*imageFrom.cols/imageFrom.rows*2)*(viewer?2:1));
+			dialog.setMinimumHeight(640);
+			layout = new QHBoxLayout();
+			vertical = false;
+		}
+
+		ImageView * viewA = new ImageView(&dialog);
+		ImageView * viewB = new ImageView(&dialog);
+
+		layout->setSpacing(0);
+		layout->addWidget(viewA, 1);
+		layout->addWidget(viewB, 1);
+
+		mainLayout->addLayout(layout, 1);
+		if(viewer)
+		{
+			mainLayout->addWidget(viewer, 1);
+		}
+
+		dialog.setLayout(mainLayout);
+
+		dialog.show();
+
+		viewA->setImage(uCvMat2QImage(imageFrom));
+		viewA->setAlpha(200);
+		if(!fromDepth.empty())
+		{
+			viewA->setImageDepth(uCvMat2QImage(fromDepth, false, uCvQtDepthRedToBlue));
+			viewA->setImageDepthShown(true);
+		}
+		viewB->setImage(uCvMat2QImage(imageTo));
+		viewB->setAlpha(200);
+		if(!toDepth.empty())
+		{
+			viewB->setImageDepth(uCvMat2QImage(toDepth, false, uCvQtDepthRedToBlue));
+			viewB->setImageDepthShown(true);
+		}
+		viewA->setFeatures(dataFrom.getWords());
+		viewB->setFeatures(dataTo.getWords());
+		std::set<int> inliersSet(info.inliersIDs.begin(), info.inliersIDs.end());
+
+		const QMultiMap<int, KeypointItem*> & wordsA = viewA->getFeatures();
+		const QMultiMap<int, KeypointItem*> & wordsB = viewB->getFeatures();
+		if(wordsA.size() && wordsB.size())
+		{
+			QList<int> ids = wordsA.uniqueKeys();
+			for(int i=0; i<ids.size(); ++i)
+			{
+				if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
+				{
+					// Add lines
+					// Draw lines between corresponding features...
+					float scaleAX = viewA->viewScale();
+					float scaleBX = viewB->viewScale();
+
+					float scaleDiff = viewA->viewScale() / viewB->viewScale();
+					float deltaAX = 0;
+					float deltaAY = 0;
+
+					if(vertical)
+					{
+						deltaAY = viewA->height()/scaleAX;
+					}
+					else
+					{
+						deltaAX = viewA->width()/scaleAX;
+					}
+
+					float deltaBX = 0;
+					float deltaBY = 0;
+
+					if(vertical)
+					{
+						deltaBY = viewB->height()/scaleBX;
+					}
+					else
+					{
+						deltaBX = viewA->width()/scaleBX;
+					}
+
+					const KeypointItem * kptA = wordsA.value(ids[i]);
+					const KeypointItem * kptB = wordsB.value(ids[i]);
+
+					QColor cA = viewA->getDefaultMatchingLineColor();
+					QColor cB = viewB->getDefaultMatchingLineColor();
+					if(inliersSet.find(ids[i])!=inliersSet.end())
+					{
+						cA = viewA->getDefaultMatchingFeatureColor();
+						cB = viewB->getDefaultMatchingFeatureColor();
+						viewA->setFeatureColor(ids[i], viewA->getDefaultMatchingFeatureColor());
+						viewB->setFeatureColor(ids[i], viewB->getDefaultMatchingFeatureColor());
+					}
+					else
+					{
+						viewA->setFeatureColor(ids[i], viewA->getDefaultMatchingLineColor());
+						viewB->setFeatureColor(ids[i], viewB->getDefaultMatchingLineColor());
+					}
+
+					viewA->addLine(
+							kptA->rect().x()+kptA->rect().width()/2,
+							kptA->rect().y()+kptA->rect().height()/2,
+							kptB->rect().x()/scaleDiff+kptB->rect().width()/scaleDiff/2+deltaAX,
+							kptB->rect().y()/scaleDiff+kptB->rect().height()/scaleDiff/2+deltaAY,
+							cA);
+
+					viewB->addLine(
+							kptA->rect().x()*scaleDiff+kptA->rect().width()*scaleDiff/2-deltaBX,
+							kptA->rect().y()*scaleDiff+kptA->rect().height()*scaleDiff/2-deltaBY,
+							kptB->rect().x()+kptB->rect().width()/2,
+							kptB->rect().y()+kptB->rect().height()/2,
+							cB);
+				}
+			}
+			viewA->update();
+			viewB->update();
+		}
+
+		printf("Transform: %s\n", t.prettyPrint().c_str());
+		printf("Features: from=%d to=%d\n", (int)dataFrom.getWords().size(), (int)dataTo.getWords().size());
+		printf("Matches: %d\n", info.matches);
+		printf("Inliers: %d (%s=%d)\n", info.inliers, Parameters::kVisMinInliers().c_str(), reg.getMinInliers());
+		app.exec();
+		delete viewer;
+	}
+	else
+	{
+		printf("Failed loading images %s and %s\n!", argv[argc-2], argv[argc-1]);
+	}
+
+
+	return 0;
+}
+

tools/OdometryViewer/main.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/core/Odometry.h>
+#include <rtabmap/core/odometry/OdometryMono.h>
 #include <rtabmap/core/OdometryThread.h>
 #include <rtabmap/gui/OdometryViewer.h>
 #include <rtabmap/core/CameraThread.h>
@@ -47,9 +48,23 @@ void showUsage()
 	printf("\nUsage:\n"
 			"odometryViewer [options]\n"
 			"Options:\n"
-			"  -driver #                 Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=dc1394, 7=FlyCapture2\n"
+			"  -driver #                 Driver number to use: \n"
+			"                                     0=OpenNI-PCL (Kinect)\n"
+			"                                     1=OpenNI2    (Kinect and Xtion PRO Live)\n"
+			"                                     2=Freenect   (Kinect)\n"
+			"                                     3=OpenNI-CV  (Kinect)\n"
+			"                                     4=OpenNI-CV-ASUS (Xtion PRO Live)\n"
+			"                                     5=Freenect2  (Kinect v2)\n"
+			"                                     6=DC1394     (Bumblebee2)\n"
+			"                                     7=FlyCapture2 (Bumblebee2)\n"
+			"                                     8=ZED stereo\n"
+			"                                     9=RealSense\n"
+			"                                     10=Kinect for Windows 2 SDK\n"
+			"                                     11=RealSense2\n"
+			"                                     12=Kinect for Azure SDK\n"
+			"                                     13=MYNT EYE S\n"
 			"  -hz #.#                   Camera rate (default 0, 0 means as fast as the camera can)\n"
-			"  -db \"input.db\"          Use database instead of camera (recorded with rtabmap-dataRecorder)\n"
+			"  -db \"input.db\"            Use database instead of camera (recorded with rtabmap-dataRecorder)\n"
 			"  -clouds #                 Maximum clouds shown (default 10, zero means inf)\n"
 			"  -sec #.#                  Delay (seconds) before reading the database (if set)\n"
 			"%s\n",
@@ -77,7 +92,7 @@ int main (int argc, char * argv[])
 			if(i < argc)
 			{
 				driver = std::atoi(argv[i]);
-				if(driver < 0 || driver > 7)
+				if(driver < 0 || driver > 13)
 				{
 					showUsage();
 				}
@@ -184,7 +199,9 @@ int main (int argc, char * argv[])
 
 	bool icp = false;
 	int regStrategy = rtabmap::Parameters::defaultRegStrategy();
+	int odomStrategy = rtabmap::Parameters::defaultOdomStrategy();
 	rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kRegStrategy(), regStrategy);
+	rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kOdomStrategy(), odomStrategy);
 	int decimation = 8;
 	float maxDepth = 4.0f;
 	float voxelSize = rtabmap::Parameters::defaultIcpVoxelSize();
@@ -214,7 +231,17 @@ int main (int argc, char * argv[])
 
 	QApplication app(argc, argv);
 
-	rtabmap::Odometry * odom = rtabmap::Odometry::create(parameters);
+	rtabmap::Odometry * odom;
+	if(odomStrategy == -1)
+	{
+		// experimental mono
+		odom = new rtabmap::OdometryMono(parameters);
+	}
+	else
+	{
+		odom = rtabmap::Odometry::create(parameters);
+	}
+
 
 	rtabmap::OdometryThread odomThread(odom);
 	rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
@@ -298,6 +325,60 @@ int main (int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraStereoFlyCapture2(rate, t);
 	}
+	else if(driver == 8)
+	{
+		if(!rtabmap::CameraStereoZed::available())
+		{
+			UERROR("Not built with ZED sdk support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoZed(0,3,1,0,100,false,rate,t);
+	}
+	else if (driver == 9)
+	{
+		if (!rtabmap::CameraRealSense::available())
+		{
+			UERROR("Not built with RealSense support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraRealSense(0, 0, 0, false, rate, t);
+	}
+	else if (driver == 10)
+	{
+		if (!rtabmap::CameraK4W2::available())
+		{
+			UERROR("Not built with Kinect for Windows 2 SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraK4W2(0, rtabmap::CameraK4W2::kTypeDepth2ColorSD, rate, t);
+	}
+	else if (driver == 11)
+	{
+		if (!rtabmap::CameraRealSense2::available())
+		{
+			UERROR("Not built with RealSense2 SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraRealSense2("", rate, t);
+	}
+	else if (driver == 12)
+	{
+		if (!rtabmap::CameraK4A::available())
+		{
+			UERROR("Not built with Kinect for Azure SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraK4A();
+	}
+	else if (driver == 13)
+	{
+		if (!rtabmap::CameraMyntEye::available())
+		{
+			UERROR("Not built with Mynt Eye S support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraMyntEye("", false, false, rate, t);
+	}
 	else
 	{
 		UFATAL("Camera driver (%d) not found!", driver);

tools/Report/main.cpp

@@ -65,12 +65,15 @@ void showUsage()
 			"                         and valid ground truth indices to [path]_indices.txt \n"
 #ifdef WITH_QT
 			"    --stats            Show available statistics to plot (if path is a file). \n"
-			"    --invert           Put all curves from a same database in same figure, \n"
-			"                         instead of all same curves from different database \n"
-			"                         in same figure. \n"
+			"    --invert           When reading many databases, put all curves from a same \n"
+			"                       database in same figure, instead of all same curves from \n"
+			"                       different database in same figure. When reading a single \n"
+			"                       database, the inverse will be done. \n"
 			"    --ids              Use IDs for x axis instead of time in the figures. \n"
+			"    --start #          Start from this node ID for the figures.\n"
 #endif
-			"    --report           Export all statistics values in report.txt \n\n");
+			"    --report           Export all statistics values in report.txt \n"
+			"    --help             Show usage\n\n");
 	exit(1);
 }
 
@@ -97,13 +100,18 @@ int main(int argc, char * argv[])
 	bool showAvailableStats = false;
 	bool invertFigures = false;
 	bool useIds = false;
+	int startId = 0;
 	std::vector<std::string> statsToShow;
 #ifdef WITH_QT
 	std::map<std::string, UPlot*> figures;
 #endif
 	for(int i=1; i<argc-1; ++i)
 	{
-		if(strcmp(argv[i], "--latex") == 0)
+		if(strcmp(argv[i], "--help") == 0)
+		{
+			showUsage();
+		}
+		else if(strcmp(argv[i], "--latex") == 0)
 		{
 			outputLatex = true;
 		}
@@ -143,15 +151,37 @@ int main(int argc, char * argv[])
 		{
 			useIds = true;
 		}
+#ifdef WITH_QT
+		else if(strcmp(argv[i],"--start") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				startId = atoi(argv[i]);
+				printf("Figures will be plotted from id=%d (--start)\n", startId);
+			}
+			else
+			{
+				printf("Missing id for \"--start\" option.\n");
+				showUsage();
+			}
+		}
 		else
 		{
-#ifdef WITH_QT
+
 			statsToShow.push_back(argv[i]);
-#endif
 		}
+#endif
 	}
 
+	std::string path = argv[argc-1];
+	path = uReplaceChar(path, '~', UDirectory::homeDir());
+
 #ifdef WITH_QT
+	if(!UDirectory::exists(path) && UFile::getExtension(path).compare("db") == 0)
+	{
+		invertFigures = !invertFigures;
+	}
 	if(!invertFigures)
 	{
 		for(size_t i=0; i<statsToShow.size(); ++i)
@@ -159,7 +189,8 @@ int main(int argc, char * argv[])
 			std::string figureTitle = statsToShow[i];
 			printf("Plot %s\n", figureTitle.c_str());
 			UPlot * fig = new UPlot();
-			fig->setTitle(figureTitle.c_str());
+			fig->resize(QSize(640,480));
+			fig->setWindowTitle(figureTitle.c_str());
 			if(useIds)
 			{
 				fig->setXLabel("Node ID");
@@ -174,9 +205,6 @@ int main(int argc, char * argv[])
 	}
 #endif
 
-	std::string path = argv[argc-1];
-	path = uReplaceChar(path, '~', UDirectory::homeDir());
-
 	std::string fileName;
 	std::list<std::string> paths;
 	paths.push_back(path);
@@ -269,15 +297,18 @@ int main(int argc, char * argv[])
 						std::map<std::string, int> availableStats;
 						for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 						{
-							for(std::map<std::string, float>::iterator jter=stats.at(*iter).first.begin(); jter!=stats.at(*iter).first.end(); ++jter)
+							if(stats.find(*iter) != stats.end())
 							{
-								if(availableStats.find(jter->first) != availableStats.end())
+								for(std::map<std::string, float>::iterator jter=stats.at(*iter).first.begin(); jter!=stats.at(*iter).first.end(); ++jter)
 								{
-									++availableStats.at(jter->first);
-								}
-								else
-								{
-									availableStats.insert(std::make_pair(jter->first, 1));
+									if(availableStats.find(jter->first) != availableStats.end())
+									{
+										++availableStats.at(jter->first);
+									}
+									else
+									{
+										availableStats.insert(std::make_pair(jter->first, 1));
+									}
 								}
 							}
 						}
@@ -302,7 +333,7 @@ int main(int argc, char * argv[])
 					else
 					{
 						UPlot * fig = new UPlot();
-						fig->setTitle(filePath.c_str());
+						fig->setWindowTitle(filePath.c_str());
 						if(useIds)
 						{
 							fig->setXLabel("Node ID");
@@ -402,7 +433,7 @@ int main(int argc, char * argv[])
 #ifdef WITH_QT
 								for(std::map<std::string, UPlotCurve*>::iterator jter=curves.begin(); jter!=curves.end(); ++jter)
 								{
-									if(uContains(stat, jter->first))
+									if(uContains(stat, jter->first) && *iter >= startId)
 									{
 										if(!uContains(firstStamps, jter->first))
 										{

tools/Reprocess/main.cpp

@@ -78,16 +78,16 @@ void sighandler(int sig)
 }
 
 int loopCount = 0;
+int proxCount = 0;
 int totalFrames = 0;
 std::vector<float> previousLocalizationDistances;
 std::vector<float> odomDistances;
-std::vector<float> localizationDistances;
+std::vector<float> localizationVariations;
+std::vector<float> localizationAngleVariations;
 std::vector<float> localizationTime;
 void showLocalizationStats()
 {
-	printf("Total localizations on previous session = %d/%d\n", loopCount, totalFrames);
-	loopCount = 0;
-	totalFrames = 0;
+	printf("Total localizations on previous session = %d/%d (Loop=%d, Prox=%d)\n", loopCount+proxCount, totalFrames, loopCount, proxCount);
 	{
 		float m = uMean(localizationTime);
 		float var = uVariance(localizationTime, m);
@@ -98,16 +98,29 @@ void showLocalizationStats()
 		}
 		printf("Average localization time = %f ms (stddev=%f ms)\n", m, stddev);
 	}
-	if(!localizationDistances.empty())
+	if(localizationVariations.size()>=2)
 	{
-		float m = uMean(localizationDistances);
-		float var = uVariance(localizationDistances, m);
+		//ignore first localization
+		localizationVariations = std::vector<float>(++localizationVariations.begin(), localizationVariations.end());
+		localizationAngleVariations = std::vector<float>(++localizationAngleVariations.begin(), localizationAngleVariations.end());
+
+		float m = uMean(localizationVariations);
+		float max = uMax(localizationVariations);
+		float var = uVariance(localizationVariations, m);
 		float stddev = -1;
 		if(var>0)
 		{
 			stddev = sqrt(var);
 		}
-		printf("Average localization distance = %f m (stddev=%f m)\n", m, stddev);
+		float mA = uMean(localizationAngleVariations);
+		float maxA = uMax(localizationAngleVariations);
+		float varA = uVariance(localizationAngleVariations, mA);
+		float stddevA = -1;
+		if(varA>0)
+		{
+			stddevA = sqrt(varA);
+		}
+		printf("Average localization variations = %f m, %f deg (stddev=%f m, %f deg) (max=%f m, %f deg)\n", m, mA, stddev, stddevA, max, maxA);
 	}
 	if(!previousLocalizationDistances.empty())
 	{
@@ -131,6 +144,15 @@ void showLocalizationStats()
 		}
 		printf("Average odometry distances = %f m (stddev=%f m)\n", m, stddev);
 	}
+
+	loopCount = 0;
+	proxCount = 0;
+	totalFrames = 0;
+	previousLocalizationDistances.clear();
+	odomDistances.clear();
+	localizationVariations.clear();
+	localizationAngleVariations.clear();
+	localizationTime.clear();
 }
 
 int main(int argc, char * argv[])
@@ -358,7 +380,7 @@ int main(int argc, char * argv[])
 	int processed = 0;
 	CameraInfo info;
 	SensorData data = dbReader.takeImage(&info);
-	Transform lastLocalizationPose = info.odomPose;
+	Transform lastLocalizationOdomPose = info.odomPose;
 	while(data.isValid() && g_loopForever)
 	{
 		UTimer iterationTime;
@@ -375,7 +397,7 @@ int main(int argc, char * argv[])
 				if(!incrementalMemory && processed>0)
 				{
 					showLocalizationStats();
-					lastLocalizationPose = info.odomPose;
+					lastLocalizationOdomPose = info.odomPose;
 				}
 				if(incrementalMemory)
 				{
@@ -477,9 +499,16 @@ int main(int argc, char * argv[])
 		++totalFrames;
 		if (loopId>0)
 		{
-			++loopCount;
+			if(stats.loopClosureId()>0)
+			{
+				++loopCount;
+			}
+			else
+			{
+				++proxCount;
+			}
 			int loopMapId = stats.loopClosureId() > 0? stats.loopClosureMapId(): stats.proximityDetectionMapId();
-			printf("Processed %d/%d nodes [id=%d map=%d]... %dms Loop on %d [%d]\n", ++processed, totalIds, refId, refMapId, int(iterationTime.ticks() * 1000), loopId, loopMapId);
+			printf("Processed %d/%d nodes [id=%d map=%d]... %dms %s on %d [%d]\n", ++processed, totalIds, refId, refMapId, int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
 		}
 		else if(landmarkId != 0)
 		{
@@ -492,22 +521,23 @@ int main(int argc, char * argv[])
 
 		// Here we accumulate statistics about distance from last localization
 		if(!incrementalMemory &&
-		   !lastLocalizationPose.isNull() &&
+		   !lastLocalizationOdomPose.isNull() &&
 		   !info.odomPose.isNull())
 		{
 			if(loopId>0 || landmarkId != 0)
 			{
-				previousLocalizationDistances.push_back(lastLocalizationPose.getDistance(info.odomPose));
-				lastLocalizationPose = info.odomPose;
+				previousLocalizationDistances.push_back(lastLocalizationOdomPose.getDistance(info.odomPose));
+				lastLocalizationOdomPose = info.odomPose;
 			}
 		}
 		if(!incrementalMemory)
 		{
 			float totalTime = uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
 			localizationTime.push_back(totalTime);
-			if(loopId>0)
+			if(stats.data().find(Statistics::kLoopOdom_correction_norm()) != stats.data().end())
 			{
-				localizationDistances.push_back(stats.loopClosureTransform().getNorm());
+				localizationVariations.push_back(stats.data().at(Statistics::kLoopOdom_correction_norm()));
+				localizationAngleVariations.push_back(stats.data().at(Statistics::kLoopOdom_correction_angle()));
 			}
 		}
 
@@ -528,7 +558,7 @@ int main(int argc, char * argv[])
 	}
 	else
 	{
-		printf("Total loop closures = %d\n", loopCount);
+		printf("Total loop closures = %d (Loop=%d, Prox=%d)\n", loopCount+proxCount, loopCount, proxCount);
 	}
 
 	printf("Closing database \"%s\"...\n", outputDatabasePath.c_str());