Adding group option for structures exportation - moving motion detection function in utiles module - removing stilldetect module - correction in utiles

Author: nils-hamel

src/framework-database.cpp

@@ -622,7 +622,11 @@ void Database::filterDisparity(int pipeState){
 
 }
 
-void Database::exportStructure(std::string path, std::string mode, unsigned int major){
+//
+//  Section - Exportation methods
+//
+
+void Database::exportStructure(std::string path, std::string mode, unsigned int major, unsigned int group){
     std::fstream exportStream;
     std::stringstream filePath;
     std::stringstream fileCopy;
@@ -636,16 +640,20 @@ void Database::exportStructure(std::string path, std::string mode, unsigned int
         return;
     }
 
-    for(auto & element: structures){
-        exportStream << (*element->getPosition())(0) << " ";
-        exportStream << (*element->getPosition())(1) << " ";
-        exportStream << (*element->getPosition())(2) << " ";
+    for(auto & structure: structures){
+        if(structure->getHasScale(group)){
+
+            exportStream << (*structure->getPosition())(0) << " ";
+            exportStream << (*structure->getPosition())(1) << " ";
+            exportStream << (*structure->getPosition())(2) << " ";
 
-        color=element->getColor();
+            color=structure->getColor();
 
-        exportStream << std::to_string( color[2] ) << " ";
-        exportStream << std::to_string( color[1] ) << " ";
-        exportStream << std::to_string( color[0] ) << std::endl;
+            exportStream << std::to_string( color[2] ) << " ";
+            exportStream << std::to_string( color[1] ) << " ";
+            exportStream << std::to_string( color[0] ) << std::endl;
+
+        }
     }
 
     exportStream.close();
@@ -717,7 +725,7 @@ void Database::exportTransformation(std::string path, std::string mode, unsigned
 }
 
 //
-//  development related features
+//  Section - Development features - Will be removed
 //
 
 // Note : this function does not respect encapsulation (development function) - need to be removed

src/framework-database.hpp

@@ -105,7 +105,7 @@ class Database {
     void computeDisparityStatistics(int loopState);
     void filterRadialRange(int loopState);
     void filterDisparity(int loopState);
-    void exportStructure(std::string path, std::string mode, unsigned int major);
+    void exportStructure(std::string path, std::string mode, unsigned int major, unsigned int group);
     void exportPosition(std::string path, std::string mode, unsigned int major);
     void exportTransformation(std::string path, std::string mode, unsigned int major);
     Structure *newStructure(){ auto s = std::make_shared<Structure>(); structures.push_back(s); return s.get();}

src/framework-frontend.cpp

@@ -1,6 +1,4 @@
 #include "framework-frontend.hpp"
-
-#include "framework-stillcompute.hpp"
 #include "framework-sparsefeature.hpp"
 #include "framework-utiles.hpp"
 
@@ -45,7 +43,7 @@ bool FrontendPicture::next() {
 			lastViewpoint->getImage()->size(),
 			&lastViewpointMatches
 		);
-		double score = computeStillDistance(
+		double score = utilesDetectMotion(
 			newViewpoint->getCvFeatures(),
 			lastViewpoint->getCvFeatures(),
 			&lastViewpointMatches,

src/framework-stillcompute.cpp

@@ -24,11 +24,14 @@
 Eigen::Vector3d utiles_direction(double x, double y, int width, int height){
 
     // Convert pixel coordinates to geographic mapping coordinates
-    double lamda( (x/width) * 2.* M_PI );
-    double phi  ( ((y/(height-1)) - 0.5) * M_PI );
+    double lamda((x/width)*2.*M_PI);
+    double phi(((y/(height-1))-0.5)*M_PI);
+
+    // Precompute cosine value
+    double cosine(cos(phi));
 
     // Compute and return direction vector
-    return Eigen::Vector3d(cos(phi)*cos(lamda), cos(phi)*sin(lamda), sin(phi));
+    return Eigen::Vector3d(cosine*cos(lamda),cosine*sin(lamda),sin(phi));
 
 }
 
@@ -51,24 +54,25 @@ void profile(std::string msg){
     msgLast = msg;
 }
 
+/* delete */
 std::mutex exitMutex;
 int exitCounter = 0;
 
+/* delete */
 void exitRetain(){
     exitMutex.lock();
     exitCounter++;
     exitMutex.unlock();
 }
 
-
+/* delete */
 void exitRelease(){
     exitMutex.lock();
     exitCounter--;
     if(exitCounter <= 0) exit(0);
     exitMutex.unlock();
 }
 
-
 double bilinear_sample(double *p, double x, double y, int width){
     int ix = x;
     int iy = y;
@@ -122,3 +126,52 @@ void utiles_directories( std::string rootPath, std::string modeName ) {
 
 }
 
+double utilesDetectMotion(std::vector<cv::KeyPoint> *kp1, std::vector<cv::KeyPoint> *kp2, std::vector<cv::DMatch> *matches, cv::Size size) {
+
+	// Vector containing the distances for each pair of features
+	std::vector<double> vec_dist {};
+
+	// loop over the vector of matches
+	for (const auto &m : *matches) {
+
+		auto width = size.width;
+		auto height = size.height;
+
+		// m.queryIdx is the index of the Keypoints on the first image
+		// m.trainIdx is the index of the Keypoints on the second image
+
+		// Convert cartesian to spherical
+		// Spherical coordinate of the feature on the first image
+		Eigen::Vector3d  p1 = utiles_direction(static_cast<double>((*kp1)[m.queryIdx].pt.x), static_cast<double>((*kp1)[m.queryIdx].pt.y), width, height);
+		// Spherical coordinate of the feature on the second image
+		Eigen::Vector3d  p2 = utiles_direction(static_cast<double>((*kp2)[m.trainIdx].pt.x), static_cast<double>((*kp2)[m.trainIdx].pt.y), width, height);
+
+		// p1*p2 computes the dot product between p1 and p2
+		// push it to vector of distances
+		vec_dist.push_back(acos(p1.dot(p2)));
+
+	}
+
+	// calculation of the mean value
+	double m { std::accumulate(vec_dist.begin(), vec_dist.end(), 0.0) / vec_dist.size() };
+
+	// calculation of variance
+	double var { 0.0 };
+	for (const auto & val : vec_dist) {
+		var += pow(val - m, 2);
+	}
+
+	// check error to avoid division by 0
+	if (vec_dist.size() <= 1) {
+		throw(std::runtime_error("Number of features is less or equal to 1"));
+	}
+	var /= (vec_dist.size() - 1);
+
+	// calculation of std
+	double vec_std = sqrt(var);
+
+	// return mean * std
+	return m * vec_std;
+
+}
+

src/framework-stillcompute.hpp

@@ -23,6 +23,7 @@
 
 // External includes
 #include <Eigen/Core>
+#include <cmath>
 #include <string>
 #include <time.h>
 #include <iostream>
@@ -33,6 +34,8 @@
 #include <experimental/filesystem>
 #include <sstream>
 #include <iomanip>
+#include <opencv4/opencv2/core.hpp>
+#include <opencv4/opencv2/core/types.hpp>
 
 // Namespaces
 namespace fs = std::experimental::filesystem;
@@ -126,3 +129,5 @@ double bilinear_sample(float *p, double x, double y, int width);
 
 void utiles_directories( std::string rootPath, std::string modeName );
 
+double utilesDetectMotion(std::vector<cv::KeyPoint> *kp1, std::vector<cv::KeyPoint> *kp2, std::vector<cv::DMatch> *matches, cv::Size size);
+

src/framework-utiles.cpp

@@ -287,7 +287,7 @@ int main(int argc, char ** argv){
         }
 
         // Major iteration exportation : model, odometry and transformation
-        database.exportStructure     (yamlExport["path"].as<std::string>(),yamlFrontend["type"].as<std::string>(),loopMajor);
+        database.exportStructure     (yamlExport["path"].as<std::string>(),yamlFrontend["type"].as<std::string>(),loopMajor,yamlExport["group"].as<unsigned int>());
         database.exportPosition      (yamlExport["path"].as<std::string>(),yamlFrontend["type"].as<std::string>(),loopMajor);
         database.exportTransformation(yamlExport["path"].as<std::string>(),yamlFrontend["type"].as<std::string>(),loopMajor);
 

src/framework-utiles.hpp

@@ -32,9 +32,7 @@
 #include "framework-database.hpp"
 #include "framework-viewpointsource.hpp"
 #include "framework-sparsefeature.hpp"
-#include "framework-stillcompute.hpp"
 #include "framework-frontend.hpp"
-#include "framework-stillcompute.hpp"
 #include "framework-utiles.hpp"
 #include "ThreadPool.h"