Similar

.gitignore

@@ -17,6 +17,7 @@ libraries/setup/boost_1_58_0/
 libraries/tbb43_20150611oss/
 libraries/vicon/
 libraries/*.log
+modules/infinitam/
 *.tags
 tags
 tests/mike/

.vscode/settings.json

@@ -0,0 +1,73 @@
+{
+    "files.associations": {
+        "xstring": "cpp",
+        "memory": "cpp",
+        "*.tpp": "cpp",
+        "*.tcu": "cpp",
+        "algorithm": "cpp",
+        "atomic": "cpp",
+        "bit": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "compare": "cpp",
+        "concepts": "cpp",
+        "cstddef": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "deque": "cpp",
+        "exception": "cpp",
+        "fstream": "cpp",
+        "functional": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "ios": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "iterator": "cpp",
+        "limits": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "new": "cpp",
+        "numeric": "cpp",
+        "ostream": "cpp",
+        "queue": "cpp",
+        "set": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "string": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "typeinfo": "cpp",
+        "unordered_map": "cpp",
+        "utility": "cpp",
+        "vector": "cpp",
+        "xfacet": "cpp",
+        "xhash": "cpp",
+        "xiosbase": "cpp",
+        "xlocale": "cpp",
+        "xlocinfo": "cpp",
+        "xlocmon": "cpp",
+        "xlocnum": "cpp",
+        "xloctime": "cpp",
+        "xmemory": "cpp",
+        "xstddef": "cpp",
+        "xtr1common": "cpp",
+        "xtree": "cpp",
+        "xutility": "cpp",
+        "ratio": "cpp",
+        "stop_token": "cpp",
+        "thread": "cpp",
+        "condition_variable": "cpp",
+        "cstdarg": "cpp",
+        "mutex": "cpp",
+        "unordered_set": "cpp"
+    }
+}

apps/spaintgui/Application.cpp

@@ -5,6 +5,11 @@
 
 #include "Application.h"
 using namespace tvginput;
+#include <time.h>  
+#include <sys/types.h>
+#include <sys/wait.h>
+#include <stdio.h>
+#include <unistd.h>
 
 #include <fstream>
 #include <stdexcept>
@@ -51,13 +56,14 @@ using namespace tvgutil;
 
 //#################### CONSTRUCTORS ####################
 
-Application::Application(const MultiScenePipeline_Ptr& pipeline, bool renderFiducials)
+Application::Application(const MultiScenePipeline_Ptr& pipeline, const std::map<std::string, std::string> &sceneID2Name, bool renderFiducials)
 : m_activeSubwindowIndex(0),
   m_batchModeEnabled(false),
   m_commandManager(10),
   m_pauseBetweenFrames(true),
   m_paused(true),
   m_pipeline(pipeline),
+  m_sceneID2Name(sceneID2Name),
   m_renderFiducials(renderFiducials),
   m_saveModelsOnExit(false),
   m_usePoseMirroring(true),
@@ -84,8 +90,16 @@ Application::Application(const MultiScenePipeline_Ptr& pipeline, bool renderFidu
 
 bool Application::run()
 {
+  time_t start, end;
+  double cost = 0;
+  time(&start);
   for(int i=0;;i++)
-  {
+  { 
+	time(&end);
+	cost = difftime(end, start);
+	if (cost>30.0) {
+		break;
+	}
     // Check to see if the user wants to quit the application, and quit if necessary. Note that if we
     // are running in batch mode, we quit directly, rather than saving a mesh of the scene on exit.
     bool eventQuit = !process_events();
@@ -98,7 +112,6 @@ bool Application::run()
 
     // Take action as relevant based on the current input state.
     process_input();
-
     // If the application is unpaused, process a new frame.
     if(!m_paused)
     {
@@ -118,8 +131,9 @@ bool Application::run()
         break;
       }
     }
-
+	usleep(100000);
     // Render the scene.
+    /*
     m_renderer->render(m_fracWindowPos, m_renderFiducials);
 
     // If we're running a mapping server and we want to render any scene images requested by remote clients, do so.
@@ -135,13 +149,14 @@ bool Application::run()
 
     // If desired, pause at the end of each frame for debugging purposes.
     if(m_pauseBetweenFrames) m_paused = true;
+    */
   }
 
   // If desired, save a mesh of the scene before the application terminates.
   if(m_saveMeshOnExit) save_mesh();
 
   // If desired, save a model of each scene before the application terminates.
-  if(m_saveModelsOnExit) save_models();
+  // if(m_saveModelsOnExit) save_models();
 
   return true;
 }
@@ -959,16 +974,23 @@ void Application::save_mesh() const
   const std::vector<std::string> sceneIDs = model->get_scene_ids();
 
   // Determine the (base) filename to use for the mesh, based on either the experiment tag (if specified) or the current timestamp (otherwise).
-  std::string meshBaseName = settings->get_first_value<std::string>("experimentTag", "spaint-" + TimeUtil::get_iso_timestamp());
+  // std::string meshBaseName = settings->get_first_value<std::string>("experimentTag", "spaint-" + TimeUtil::get_iso_timestamp());
 
   // Determine the directory into which to save the meshes, and make sure that it exists.
+  /* 
   boost::filesystem::path dir = find_subdir_from_executable("meshes");
   if(sceneIDs.size() > 1) dir = dir / meshBaseName;
   boost::filesystem::create_directories(dir);
+  */
+  if (!(model->get_collaborative_pose_optimiser()->isSuccess())) {
+    return;
+  }
 
   // Mesh each scene independently.
+  std::vector<std::string> mergeFilesName;
+  std::vector<std::string> filesRootName;
   for(size_t sceneIdx = 0; sceneIdx < sceneIDs.size(); ++sceneIdx)
-  {
+  { 
     const std::string& sceneID = sceneIDs[sceneIdx];
     std::cout << "Meshing " << sceneID << " scene.\n";
     SpaintVoxelScene_CPtr scene = model->get_slam_state(sceneID)->get_voxel_scene();
@@ -1011,6 +1033,7 @@ void Application::save_mesh() const
 
       // Next, transform each triangle using the relative transform determined above.
       const Matrix4f transform = *relativeTransform;
+	  std::cout << "transform:\n" << transform << "\n";
       Triangle *trianglesData = triangles->GetData(MEMORYDEVICE_CPU);
       for(size_t triangleIdx = 0; triangleIdx < mesh->noTotalTriangles; ++triangleIdx)
       {
@@ -1024,10 +1047,37 @@ void Application::save_mesh() const
     }
 
     // Save the mesh to disk.
-    const boost::filesystem::path meshPath = dir / (meshBaseName + "_" + sceneID + ".ply");
+    // const boost::filesystem::path meshPath = dir / (meshBaseName + "_" + sceneID + ".ply");
+    auto sceneName = m_sceneID2Name.find(sceneID);
+    const boost::filesystem::path meshPath = sceneName->second + "-" + TimeUtil::get_iso_timestamp() + ".ply";
+    mergeFilesName.push_back(meshPath.string());
     std::cout << "Saving mesh to: " << meshPath << '\n';
     mesh->WritePLY(meshPath.string().c_str());
   }
+  std::string worldScene = m_sceneID2Name.find("World")->second;
+  std::string localScene = m_sceneID2Name.find("Local1")->second;
+  std::string outputFile = worldScene + "-" + localScene + ".ply";
+  pid_t pid = fork();
+  if (pid < 0) {
+    printf("error create merge program\n");
+    exit(0);
+  }
+  if (pid == 0) {
+    std::cout << "fork python program " << mergeFilesName[0] << " " << mergeFilesName[1] << "\n";
+	char *argv[] = {"python3","mergeMesh.py", 
+					"--file1", const_cast<char*>(mergeFilesName[0].c_str()),
+					"--file2",const_cast<char*>(mergeFilesName[1].c_str()),
+					"--pose", "worldPose.txt",
+					"--output", const_cast<char*>(outputFile.c_str()),
+					NULL};
+	execvp("python3", argv);
+	std::cout << "after execlp python program\n";	
+  }else {
+	std::cout << "this is origin program\n";
+	int status;
+	waitpid(pid, &status, 0);
+	std::cout << "after waitpid\n";
+  }
 }
 
 void Application::save_models() const
@@ -1170,3 +1220,4 @@ void Application::toggle_recording(const std::string& type, boost::optional<tvgu
     std::cout << "[spaint] Started saving " << type << " to " << pathGenerator->get_base_dir() << "...\n";
   }
 }
+

apps/spaintgui/Application.h

@@ -6,6 +6,8 @@
 #ifndef H_SPAINTGUI_APPLICATION
 #define H_SPAINTGUI_APPLICATION
 
+#include <map>
+
 #include <tvgutil/boost/WrappedAsio.h>
 #include <boost/function.hpp>
 
@@ -105,6 +107,9 @@ class Application
   /** The stream of commands being sent from the voice command server. */
   boost::asio::ip::tcp::iostream m_voiceCommandStream;
 
+  /** scene name and scene ID map*/
+  std::map<std::string, std::string> m_sceneID2Name;
+
   //#################### CONSTRUCTORS ####################
 public:
   /**
@@ -113,7 +118,9 @@ class Application
    * \param pipeline        The multi-scene pipeline that the application should use.
    * \param renderFiducials Whether or not to render the fiducials (if any) that have been detected in the 3D scene.
    */
-  Application(const MultiScenePipeline_Ptr& pipeline, bool renderFiducials = false);
+  Application(const MultiScenePipeline_Ptr& pipeline, 
+              const std::map<std::string, std::string> &sceneID2Name,
+              bool renderFiducials = false);
 
   //#################### PUBLIC MEMBER FUNCTIONS ####################
 public:
@@ -356,3 +363,4 @@ class Application
 };
 
 #endif
+