From ae589c1dc9a466a2dd93ba454a76ef8b166c4225 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Wed, 3 Feb 2016 13:43:21 +0100 Subject: [PATCH 01/28] Install ORB_SLAM2 --- SuperBuild/CMakeLists.txt | 4 +- SuperBuild/cmake/External-ORB_SLAM2.cmake | 71 +++++++++++++++++++++++ SuperBuild/cmake/External-OpenCV.cmake | 4 +- SuperBuild/cmake/External-Pangolin.cmake | 29 +++++++++ configure.sh | 5 +- 5 files changed, 109 insertions(+), 4 deletions(-) create mode 100644 SuperBuild/cmake/External-ORB_SLAM2.cmake create mode 100644 SuperBuild/cmake/External-Pangolin.cmake diff --git a/SuperBuild/CMakeLists.txt b/SuperBuild/CMakeLists.txt index 152916e2..2aa28cbe 100644 --- a/SuperBuild/CMakeLists.txt +++ b/SuperBuild/CMakeLists.txt @@ -102,7 +102,9 @@ set(custom_libs OpenGV CMVS Catkin Ecto - LAStools) + LAStools + Pangolin + ORB_SLAM2) foreach(lib ${custom_libs}) SETUP_EXTERNAL_PROJECT_CUSTOM(${lib}) diff --git a/SuperBuild/cmake/External-ORB_SLAM2.cmake b/SuperBuild/cmake/External-ORB_SLAM2.cmake new file mode 100644 index 00000000..ffc0b2b6 --- /dev/null +++ b/SuperBuild/cmake/External-ORB_SLAM2.cmake @@ -0,0 +1,71 @@ +set(_proj_name orb_slam2) +set(_SB_BINARY_DIR "${SB_BINARY_DIR}/${_proj_name}") + +ExternalProject_Add(${_proj_name} + DEPENDS opencv pangolin + PREFIX ${_SB_BINARY_DIR} + TMP_DIR ${_SB_BINARY_DIR}/tmp + STAMP_DIR ${_SB_BINARY_DIR}/stamp + #--Download step-------------- + DOWNLOAD_DIR ${SB_DOWNLOAD_DIR} + URL https://github.com/raulmur/ORB_SLAM2/archive/2fc9730d9716c36b25ea1b0eba3bbd094e20f0d5.zip + URL_MD5 629f19b6d424e676ce82fa1c3a0ba43b + #--Update/Patch step---------- + UPDATE_COMMAND "" + #--Configure step------------- + SOURCE_DIR ${SB_SOURCE_DIR}/${_proj_name} + CMAKE_ARGS + -DCMAKE_INSTALL_PREFIX:PATH=${SB_INSTALL_DIR} + #--Build step----------------- + BINARY_DIR ${_SB_BINARY_DIR} + #--Install step--------------- + INSTALL_COMMAND "" + #--Output logging------------- + LOG_DOWNLOAD OFF + LOG_CONFIGURE OFF + LOG_BUILD OFF +) + +# DBoW2 +set(DBoW2_BINARY_DIR "${SB_BINARY_DIR}/DBoW2") +file(MAKE_DIRECTORY "${DBoW2_BINARY_DIR}") + +ExternalProject_Add_Step(${_proj_name} build_DBoW2 + COMMAND make -j2 + DEPENDEES configure_DBoW2 + DEPENDERS configure + WORKING_DIRECTORY ${DBoW2_BINARY_DIR} + ALWAYS 1 +) + +ExternalProject_Add_Step(${_proj_name} configure_DBoW2 + COMMAND ${CMAKE_COMMAND} /Thirdparty/DBoW2 + -DOpenCV_DIR=${SB_INSTALL_DIR}/share/OpenCV + -DCMAKE_BUILD_TYPE=Release + DEPENDEES download + DEPENDERS build_DBoW2 + WORKING_DIRECTORY ${DBoW2_BINARY_DIR} + ALWAYS 1 +) + +# g2o +set(g2o_BINARY_DIR "${SB_BINARY_DIR}/g2o") +file(MAKE_DIRECTORY "${g2o_BINARY_DIR}") + +ExternalProject_Add_Step(${_proj_name} build_g2o + COMMAND make -j2 + DEPENDEES configure_g2o + DEPENDERS configure + WORKING_DIRECTORY ${g2o_BINARY_DIR} + ALWAYS 1 +) + +ExternalProject_Add_Step(${_proj_name} configure_g2o + COMMAND ${CMAKE_COMMAND} /Thirdparty/g2o + -DCMAKE_BUILD_TYPE=Release + DEPENDEES download + DEPENDERS build_g2o + WORKING_DIRECTORY ${g2o_BINARY_DIR} + ALWAYS 1 +) + diff --git a/SuperBuild/cmake/External-OpenCV.cmake b/SuperBuild/cmake/External-OpenCV.cmake index 1ca96e61..a2c0d51f 100644 --- a/SuperBuild/cmake/External-OpenCV.cmake +++ b/SuperBuild/cmake/External-OpenCV.cmake @@ -27,7 +27,7 @@ ExternalProject_Add(${_proj_name} -DBUILD_opencv_photo=ON -DBUILD_opencv_legacy=ON -DBUILD_opencv_python=ON - -DWITH_FFMPEG=OFF + -DWITH_FFMPEG=ON -DWITH_CUDA=OFF -DWITH_GTK=OFF -DWITH_VTK=OFF @@ -57,4 +57,4 @@ ExternalProject_Add(${_proj_name} LOG_DOWNLOAD OFF LOG_CONFIGURE OFF LOG_BUILD OFF -) \ No newline at end of file +) diff --git a/SuperBuild/cmake/External-Pangolin.cmake b/SuperBuild/cmake/External-Pangolin.cmake new file mode 100644 index 00000000..9fa02345 --- /dev/null +++ b/SuperBuild/cmake/External-Pangolin.cmake @@ -0,0 +1,29 @@ +set(_proj_name pangolin) +set(_SB_BINARY_DIR "${SB_BINARY_DIR}/${_proj_name}") + +ExternalProject_Add(${_proj_name} + PREFIX ${_SB_BINARY_DIR} + TMP_DIR ${_SB_BINARY_DIR}/tmp + STAMP_DIR ${_SB_BINARY_DIR}/stamp + #--Download step-------------- + DOWNLOAD_DIR ${SB_DOWNLOAD_DIR} + URL https://github.com/paulinus/Pangolin/archive/952c62bd6adbbc8bf58c5941fbb353ce7ff9e022.zip + URL_MD5 1b8a3713a1b9da699187cf1e8dc60eab + #--Update/Patch step---------- + UPDATE_COMMAND "" + #--Configure step------------- + SOURCE_DIR ${SB_SOURCE_DIR}/${_proj_name} + CMAKE_ARGS + -DCPP11_NO_BOOST=1 + -DCMAKE_INSTALL_PREFIX:PATH=${SB_INSTALL_DIR} + + #--Build step----------------- + BINARY_DIR ${_SB_BINARY_DIR} + #--Install step--------------- + INSTALL_DIR ${SB_INSTALL_DIR} + #--Output logging------------- + LOG_DOWNLOAD OFF + LOG_CONFIGURE OFF + LOG_BUILD OFF +) + diff --git a/configure.sh b/configure.sh index 641bf744..33af7014 100644 --- a/configure.sh +++ b/configure.sh @@ -111,6 +111,9 @@ fi ## Get sys vars NUM_CORES=`grep -c processor /proc/cpuinfo` +## Add SuperBuild path to the library path +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:`pwd`/SuperBuild/install/lib + ## Add SuperBuild path to the python path export PYTHONPATH=$PYTHONPATH:`pwd`/SuperBuild/install/lib/python2.7/dist-packages @@ -119,4 +122,4 @@ cd SuperBuild mkdir -p build && cd build cmake .. && make -j ${NUM_CORES} -echo -e "\e[1;34mScript finished\e[0;39m" \ No newline at end of file +echo -e "\e[1;34mScript finished\e[0;39m" From 19e45d58cf0847f5eea6f0067cd54af4b8db4945 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Wed, 3 Feb 2016 15:58:21 +0100 Subject: [PATCH 02/28] Add odm_slam module --- modules/CMakeLists.txt | 2 +- modules/odm_slam/CMakeLists.txt | 38 +++++++++++++++++++++++++++ modules/odm_slam/src/OdmSlam.cpp | 44 ++++++++++++++++++++++++++++++++ 3 files changed, 83 insertions(+), 1 deletion(-) create mode 100644 modules/odm_slam/CMakeLists.txt create mode 100644 modules/odm_slam/src/OdmSlam.cpp diff --git a/modules/CMakeLists.txt b/modules/CMakeLists.txt index fa379225..54d4f04d 100644 --- a/modules/CMakeLists.txt +++ b/modules/CMakeLists.txt @@ -4,4 +4,4 @@ add_subdirectory(odm_georef) add_subdirectory(odm_meshing) add_subdirectory(odm_orthophoto) add_subdirectory(odm_texturing) - +add_subdirectory(odm_slam) diff --git a/modules/odm_slam/CMakeLists.txt b/modules/odm_slam/CMakeLists.txt new file mode 100644 index 00000000..df97d53c --- /dev/null +++ b/modules/odm_slam/CMakeLists.txt @@ -0,0 +1,38 @@ +project(odm_slam) +cmake_minimum_required(VERSION 2.8) + +# Set opencv dir to the input spedified with option -DOPENCV_DIR="path" +set(OPENCV_DIR "OPENCV_DIR-NOTFOUND" CACHE "OPENCV_DIR" "Path to the opencv installation directory") + +# Add compiler options. +add_definitions(-Wall -Wextra) + +# Find pcl at the location specified by PCL_DIR +find_package(PCL 1.7 HINTS "${PCL_DIR}/share/pcl-1.7" REQUIRED) + +# Find OpenCV at the default location +find_package(OpenCV HINTS "${OPENCV_DIR}" REQUIRED) + +# Only link with required opencv modules. +set(OpenCV_LIBS opencv_core opencv_imgproc opencv_highgui) + +# Add the Eigen and OpenCV include dirs. +# Necessary since the PCL_INCLUDE_DIR variable set by find_package is broken.) +include_directories(${EIGEN_ROOT}) +include_directories(${OpenCV_INCLUDE_DIRS}) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -std=c++11") + +set(ORB_SLAM_ROOT ${CMAKE_BINARY_DIR}/../SuperBuild/src/orb_slam2) + +include_directories(${EIGEN_ROOT}) +include_directories(${ORB_SLAM_ROOT}) +include_directories(${ORB_SLAM_ROOT}/include) +link_directories(${ORB_SLAM_ROOT}/lib) + +# Add source directory +aux_source_directory("./src" SRC_LIST) + +# Add exectuteable +add_executable(${PROJECT_NAME} ${SRC_LIST}) +target_link_libraries(odm_slam ${OpenCV_LIBS} ORB_SLAM2 pangolin) diff --git a/modules/odm_slam/src/OdmSlam.cpp b/modules/odm_slam/src/OdmSlam.cpp new file mode 100644 index 00000000..0b6faf6c --- /dev/null +++ b/modules/odm_slam/src/OdmSlam.cpp @@ -0,0 +1,44 @@ +#include + +#include + +#include + + +int main(int argc, char **argv) { + if(argc != 4) { + std::cerr << std::endl << + "Usage: ./mono_odm vocabulary settings video" << + std::endl; + return 1; + } + + cv::VideoCapture cap(argv[3]); + if(!cap.isOpened()) { + std::cerr << "Failed to load video: " << argv[3] << std::endl; + return -1; + } + + ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true); + + cout << "Start processing video ..." << endl; + + int T_micro = 300000; + cv::Mat im; + for(int ni = 0;; ++ni){ + cap >> im; + + if(im.empty()) { + break; + } + + SLAM.TrackMonocular(im, ni * T_micro); + + usleep(T_micro); + } + + SLAM.Shutdown(); + SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + + return 0; +} From 0b5189ed6d6a794b566feac739422ae053d557e1 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Wed, 3 Feb 2016 23:28:16 +0100 Subject: [PATCH 03/28] Use gtk for opencv highgui --- SuperBuild/cmake/External-OpenCV.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SuperBuild/cmake/External-OpenCV.cmake b/SuperBuild/cmake/External-OpenCV.cmake index a2c0d51f..0e39d6b1 100644 --- a/SuperBuild/cmake/External-OpenCV.cmake +++ b/SuperBuild/cmake/External-OpenCV.cmake @@ -29,7 +29,7 @@ ExternalProject_Add(${_proj_name} -DBUILD_opencv_python=ON -DWITH_FFMPEG=ON -DWITH_CUDA=OFF - -DWITH_GTK=OFF + -DWITH_GTK=ON -DWITH_VTK=OFF -DWITH_EIGEN=OFF -DWITH_OPENNI=OFF From 3ac213d62e7a75ec06d480cd810d1fbc31bf7e38 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Wed, 3 Feb 2016 23:28:40 +0100 Subject: [PATCH 04/28] Uncompress orb_slam vocabulary --- SuperBuild/cmake/External-ORB_SLAM2.cmake | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/SuperBuild/cmake/External-ORB_SLAM2.cmake b/SuperBuild/cmake/External-ORB_SLAM2.cmake index ffc0b2b6..d7f11ae2 100644 --- a/SuperBuild/cmake/External-ORB_SLAM2.cmake +++ b/SuperBuild/cmake/External-ORB_SLAM2.cmake @@ -69,3 +69,11 @@ ExternalProject_Add_Step(${_proj_name} configure_g2o ALWAYS 1 ) +# Uncompress Vocabulary +ExternalProject_Add_Step(${_proj_name} uncompress_vocabulary + COMMAND tar -xf ORBvoc.txt.tar.gz + DEPENDEES download + DEPENDERS configure + WORKING_DIRECTORY /Vocabulary + ALWAYS 1 +) From a53e26046e9d434dfb8b2137305644b39a0409ad Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Thu, 4 Feb 2016 11:27:45 +0100 Subject: [PATCH 05/28] Enable running slam with run.py --- opendm/config.py | 4 +++ scripts/odm_app.py | 63 ++++++++++++++++++++++++++++----------------- scripts/odm_slam.py | 44 +++++++++++++++++++++++++++++++ 3 files changed, 87 insertions(+), 24 deletions(-) create mode 100644 scripts/odm_slam.py diff --git a/opendm/config.py b/opendm/config.py index ec22ea69..60921204 100644 --- a/opendm/config.py +++ b/opendm/config.py @@ -33,6 +33,10 @@ parser.add_argument('--rerun', '-r', choices=processopts, help=('Can be one of:' + ' | '.join(processopts))) +parser.add_argument('--video', + metavar='', + help='Path to the video file to process') + parser.add_argument('--force-focal', metavar='', type=float, diff --git a/scripts/odm_app.py b/scripts/odm_app.py index 19236eb6..a9795215 100644 --- a/scripts/odm_app.py +++ b/scripts/odm_app.py @@ -7,6 +7,7 @@ from opendm import config from dataset import ODMLoadDatasetCell from resize import ODMResizeCell from opensfm import ODMOpenSfMCell +from odm_slam import ODMSlamCell from pmvs import ODMPmvsCell from cmvs import ODMCmvsCell from odm_meshing import ODMeshingCell @@ -40,6 +41,7 @@ class ODMApp(ecto.BlackBox): processes=context.num_cores, matching_gps_neighbors=p.args['matcher_neighbors'], matching_gps_distance=p.args['matcher_distance']), + 'slam': ODMSlamCell(), 'cmvs': ODMCmvsCell(max_images=p.args['cmvs_maxImages']), 'pmvs': ODMPmvsCell(level=p.args['pmvs_level'], csize=p.args['pmvs_csize'], @@ -68,6 +70,8 @@ class ODMApp(ecto.BlackBox): self.tree = ecto.Constant(value=tree) def connections(self, _p): + run_slam = _p.args.get('video') is not None + # define initial task initial_task = _p.args['start_with'] initial_task_id = config.processopts.index(initial_task) @@ -75,23 +79,33 @@ class ODMApp(ecto.BlackBox): ## define the connections like you would for the plasm connections = [] - ## load the dataset - connections = [ self.tree[:] >> self.dataset['tree'] ] + if run_slam: + # run slam cell + connections += [ self.tree[:] >> self.slam['tree'], + self.args[:] >> self.slam['args'] ] - # run resize cell - connections += [ self.tree[:] >> self.resize['tree'], - self.args[:] >> self.resize['args'], - self.dataset['photos'] >> self.resize['photos'] ] + # run cmvs + connections += [ self.tree[:] >> self.cmvs['tree'], + self.args[:] >> self.cmvs['args'], + self.slam['reconstruction'] >> self.cmvs['reconstruction'] ] + else: + # load the dataset + connections = [ self.tree[:] >> self.dataset['tree'] ] - # run opensfm with images from load dataset - connections += [ self.tree[:] >> self.opensfm['tree'], - self.args[:] >> self.opensfm['args'], - self.resize['photos'] >> self.opensfm['photos'] ] - - # run cmvs - connections += [ self.tree[:] >> self.cmvs['tree'], - self.args[:] >> self.cmvs['args'], - self.opensfm['reconstruction'] >> self.cmvs['reconstruction'] ] + # run resize cell + connections += [ self.tree[:] >> self.resize['tree'], + self.args[:] >> self.resize['args'], + self.dataset['photos'] >> self.resize['photos'] ] + + # run opensfm with images from load dataset + connections += [ self.tree[:] >> self.opensfm['tree'], + self.args[:] >> self.opensfm['args'], + self.resize['photos'] >> self.opensfm['photos'] ] + + # run cmvs + connections += [ self.tree[:] >> self.cmvs['tree'], + self.args[:] >> self.cmvs['args'], + self.opensfm['reconstruction'] >> self.cmvs['reconstruction'] ] # run pmvs connections += [ self.tree[:] >> self.pmvs['tree'], @@ -108,15 +122,16 @@ class ODMApp(ecto.BlackBox): self.args[:] >> self.texturing['args'], self.meshing['reconstruction'] >> self.texturing['reconstruction'] ] - # create odm georeference - connections += [ self.tree[:] >> self.georeferencing['tree'], - self.args[:] >> self.georeferencing['args'], - self.dataset['photos'] >> self.georeferencing['photos'], - self.texturing['reconstruction'] >> self.georeferencing['reconstruction'] ] + if not run_slam: + # create odm georeference + connections += [ self.tree[:] >> self.georeferencing['tree'], + self.args[:] >> self.georeferencing['args'], + self.dataset['photos'] >> self.georeferencing['photos'], + self.texturing['reconstruction'] >> self.georeferencing['reconstruction'] ] - ## create odm orthophoto - connections += [ self.tree[:] >> self.orthophoto['tree'], - self.args[:] >> self.orthophoto['args'], - self.georeferencing['reconstruction'] >> self.orthophoto['reconstruction'] ] + ## create odm orthophoto + connections += [ self.tree[:] >> self.orthophoto['tree'], + self.args[:] >> self.orthophoto['args'], + self.georeferencing['reconstruction'] >> self.orthophoto['reconstruction'] ] return connections diff --git a/scripts/odm_slam.py b/scripts/odm_slam.py new file mode 100644 index 00000000..5116ad5d --- /dev/null +++ b/scripts/odm_slam.py @@ -0,0 +1,44 @@ +import os + +import ecto + +from opendm import log +from opendm import system +from opendm import context + + +class ODMSlamCell(ecto.Cell): + def declare_params(self, params): + pass + + def declare_io(self, params, inputs, outputs): + inputs.declare("tree", "Struct with paths", []) + inputs.declare("args", "The application arguments.", {}) + outputs.declare("reconstruction", "list of ODMReconstructions", []) + + def process(self, inputs, outputs): + + log.ODM_INFO('Running OMD Slam Cell') + + # get inputs + tree = self.inputs.tree + args = self.inputs.args + video = os.path.join(tree.root_path, args['video']) + + if not video: + log.ODM_ERROR('No video provided') + return ecto.QUIT + + # create working directories + system.mkdir_p(tree.opensfm) + system.mkdir_p(tree.pmvs) + + # run meshing binary + system.run( + '{}/odm_slam ' + 'SuperBuild/src/orb_slam2/Vocabulary/ORBvoc.txt ' + 'SuperBuild/src/orb_slam2/Examples/Monocular/TUM1.yaml ' + '{}'.format(context.odm_modules_path, video)) + + log.ODM_INFO('Running OMD Slam Cell - Finished') + return ecto.OK if args['end_with'] != 'opensfm' else ecto.QUIT From 4f70738124543f348dc38860160773b414fe57fa Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Thu, 4 Feb 2016 11:28:44 +0100 Subject: [PATCH 06/28] Conversion from orb_slam to opensfm --- modules/odm_slam/src/orb_slam_to_opensfm.py | 96 +++++++++++++++++++++ 1 file changed, 96 insertions(+) create mode 100644 modules/odm_slam/src/orb_slam_to_opensfm.py diff --git a/modules/odm_slam/src/orb_slam_to_opensfm.py b/modules/odm_slam/src/orb_slam_to_opensfm.py new file mode 100644 index 00000000..92524c44 --- /dev/null +++ b/modules/odm_slam/src/orb_slam_to_opensfm.py @@ -0,0 +1,96 @@ +import argparse +import json +import yaml + +import cv2 +import numpy as np +from opensfm import transformations as tf + + +parser = argparse.ArgumentParser( + description='Convert ORB_SLAM2 output to OpenSfM') +parser.add_argument( + 'trajectory', + help='the trajectory file') +parser.add_argument( + 'config', + help='config file with camera calibration') +args = parser.parse_args() + + +def parse_orb_slam2_config_file(filename): + ''' + Parse ORB_SLAM2 config file. + + Parsing manually since neither pyyaml nor cv2.FileStorage seem to work. + ''' + res = {} + with open(filename) as fin: + lines = fin.readlines() + + for line in lines: + line = line.strip() + if line and line[0] != '#' and ':' in line: + key, value = line.split(':') + res[key.strip()] = value.strip() + return res + +config = parse_orb_slam2_config_file(args.config) + + +def camera_from_config(config): + ''' + Creates an OpenSfM from an ORB_SLAM2 config + ''' + fx = float(config['Camera.fx']) + fy = float(config['Camera.fy']) + cx = float(config['Camera.cx']) + cy = float(config['Camera.cy']) + k1 = float(config['Camera.k1']) + k2 = float(config['Camera.k2']) + p1 = float(config['Camera.p1']) + p2 = float(config['Camera.p2']) + width, height = 640, 480 # TODO(pau) get this from video file or keyframes + size = max(width, height) + return { + 'width': width, + 'height': height, + 'focal': np.sqrt(fx * fy) / size, + 'k1': k1, + 'k2': k2 + } + +def shots_from_trajectory(trajectory_filename): + ''' + Create opensfm shots from an orb_slam2/TUM trajectory + ''' + shots = {} + with open(trajectory_filename) as fin: + lines = fin.readlines() + + for line in lines: + a = map(float, line.split()) + timestamp = a[0] + c = np.array(a[1:4]) + q = np.array(a[4:8]) + R = tf.quaternion_matrix(q)[:3, :3] + t = -R.dot(c) + shot = { + 'camera': 'slamcam', + 'rotation': list(cv2.Rodrigues(R)[0].flat), + 'translation': list(t.flat) + } + shots['frame{:12d}.png'.format(int(timestamp))] = shot + return shots + + +r = { + 'cameras': {}, + 'shots': {} +} + +r['cameras']['slamcam'] = camera_from_config(config) +r['shots'] = shots_from_trajectory(args.trajectory) + +with open('reconstruction.json', 'w') as fout: + json.dump([r], fout, indent=4) From 233584b2a576235e9070354241999c95a4c29c81 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Thu, 4 Feb 2016 12:38:20 +0100 Subject: [PATCH 07/28] Extract keyframes from video --- modules/odm_slam/src/OdmSlam.cpp | 20 +++++--- modules/odm_slam/src/orb_slam_to_opensfm.py | 54 ++++++++++++++++++--- 2 files changed, 59 insertions(+), 15 deletions(-) diff --git a/modules/odm_slam/src/OdmSlam.cpp b/modules/odm_slam/src/OdmSlam.cpp index 0b6faf6c..dce6df3f 100644 --- a/modules/odm_slam/src/OdmSlam.cpp +++ b/modules/odm_slam/src/OdmSlam.cpp @@ -8,7 +8,7 @@ int main(int argc, char **argv) { if(argc != 4) { std::cerr << std::endl << - "Usage: ./mono_odm vocabulary settings video" << + "Usage: " << argv[0] << " vocabulary settings video" << std::endl; return 1; } @@ -19,22 +19,26 @@ int main(int argc, char **argv) { return -1; } - ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true); + ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, false); cout << "Start processing video ..." << endl; - int T_micro = 300000; + double T = 0.1; // Seconds between frames cv::Mat im; for(int ni = 0;; ++ni){ + // Get frame cap >> im; + if(im.empty()) break; - if(im.empty()) { - break; - } + // Save + double timestamp = ni * T; + keyframe_timestamps << ni << " " << timestamp << std::endl; - SLAM.TrackMonocular(im, ni * T_micro); - usleep(T_micro); + + SLAM.TrackMonocular(im, timestamp); + + usleep(int(T * 1e6)); } SLAM.Shutdown(); diff --git a/modules/odm_slam/src/orb_slam_to_opensfm.py b/modules/odm_slam/src/orb_slam_to_opensfm.py index 92524c44..954ea46c 100644 --- a/modules/odm_slam/src/orb_slam_to_opensfm.py +++ b/modules/odm_slam/src/orb_slam_to_opensfm.py @@ -1,14 +1,19 @@ import argparse import json +import os import yaml import cv2 import numpy as np from opensfm import transformations as tf +from opensfm.io import mkdir_p parser = argparse.ArgumentParser( description='Convert ORB_SLAM2 output to OpenSfM') +parser.add_argument( + 'video', + help='the tracked video file') parser.add_argument( 'trajectory', help='the trajectory file') @@ -35,13 +40,12 @@ def parse_orb_slam2_config_file(filename): res[key.strip()] = value.strip() return res -config = parse_orb_slam2_config_file(args.config) - -def camera_from_config(config): +def camera_from_config(video_filename, config_filename): ''' Creates an OpenSfM from an ORB_SLAM2 config ''' + config = parse_orb_slam2_config_file(config_filename) fx = float(config['Camera.fx']) fy = float(config['Camera.fy']) cx = float(config['Camera.cx']) @@ -50,7 +54,7 @@ def camera_from_config(config): k2 = float(config['Camera.k2']) p1 = float(config['Camera.p1']) p2 = float(config['Camera.p2']) - width, height = 640, 480 # TODO(pau) get this from video file or keyframes + width, height = get_video_size(video_filename) size = max(width, height) return { 'width': width, @@ -60,6 +64,7 @@ def camera_from_config(config): 'k2': k2 } + def shots_from_trajectory(trajectory_filename): ''' Create opensfm shots from an orb_slam2/TUM trajectory @@ -78,19 +83,54 @@ def shots_from_trajectory(trajectory_filename): shot = { 'camera': 'slamcam', 'rotation': list(cv2.Rodrigues(R)[0].flat), - 'translation': list(t.flat) + 'translation': list(t.flat), + 'created_at': timestamp, } - shots['frame{:12d}.png'.format(int(timestamp))] = shot + shots['frame{0:012d}.png'.format(int(timestamp))] = shot return shots +def get_video_size(video): + cap = cv2.VideoCapture(video) + width = int(cap.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH)) + height = int(cap.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT)) + cap.release() + return width, height + + +def extract_keyframes_from_video(video, reconstruction): + image_path = 'images' + mkdir_p(image_path) + T = 0.1 # TODO(pau) get this from config + cap = cv2.VideoCapture(video) + video_idx = 0 + + shot_ids = sorted(reconstruction['shots'].keys()) + for shot_id in shot_ids: + shot = reconstruction['shots'][shot_id] + timestamp = shot['created_at'] + keyframe_idx = int(round(timestamp / T)) + while video_idx < keyframe_idx: + ret, frame = cap.read() + video_idx += 1 + if video_idx == keyframe_idx: + print 'saving', shot_id + cv2.imwrite(os.path.join(image_path, shot_id), frame) + else: + raise RuntimeError( + 'Cound not find keyframe {} in video'.format(shot_id)) + + cap.release() + r = { 'cameras': {}, 'shots': {} } -r['cameras']['slamcam'] = camera_from_config(config) +r['cameras']['slamcam'] = camera_from_config(args.video, args.config) r['shots'] = shots_from_trajectory(args.trajectory) with open('reconstruction.json', 'w') as fout: json.dump([r], fout, indent=4) + +extract_keyframes_from_video(args.video, r) From bfa811c58e6d674a063298e02a75d558eba6ed71 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Thu, 4 Feb 2016 13:00:48 +0100 Subject: [PATCH 08/28] Fix quaternion conversion --- modules/odm_slam/src/orb_slam_to_opensfm.py | 54 +++++++++++---------- 1 file changed, 29 insertions(+), 25 deletions(-) diff --git a/modules/odm_slam/src/orb_slam_to_opensfm.py b/modules/odm_slam/src/orb_slam_to_opensfm.py index 954ea46c..aa6f5fe5 100644 --- a/modules/odm_slam/src/orb_slam_to_opensfm.py +++ b/modules/odm_slam/src/orb_slam_to_opensfm.py @@ -9,20 +9,6 @@ from opensfm import transformations as tf from opensfm.io import mkdir_p -parser = argparse.ArgumentParser( - description='Convert ORB_SLAM2 output to OpenSfM') -parser.add_argument( - 'video', - help='the tracked video file') -parser.add_argument( - 'trajectory', - help='the trajectory file') -parser.add_argument( - 'config', - help='config file with camera calibration') -args = parser.parse_args() - - def parse_orb_slam2_config_file(filename): ''' Parse ORB_SLAM2 config file. @@ -78,8 +64,8 @@ def shots_from_trajectory(trajectory_filename): timestamp = a[0] c = np.array(a[1:4]) q = np.array(a[4:8]) - R = tf.quaternion_matrix(q)[:3, :3] - t = -R.dot(c) + R = tf.quaternion_matrix([q[3], q[0], q[1], q[2]])[:3, :3].T + t = -R.dot(c) * 50 shot = { 'camera': 'slamcam', 'rotation': list(cv2.Rodrigues(R)[0].flat), @@ -99,6 +85,9 @@ def get_video_size(video): def extract_keyframes_from_video(video, reconstruction): + ''' + Reads video and extracts a frame for each shot in reconstruction + ''' image_path = 'images' mkdir_p(image_path) T = 0.1 # TODO(pau) get this from config @@ -122,15 +111,30 @@ def extract_keyframes_from_video(video, reconstruction): cap.release() -r = { - 'cameras': {}, - 'shots': {} -} -r['cameras']['slamcam'] = camera_from_config(args.video, args.config) -r['shots'] = shots_from_trajectory(args.trajectory) +if __name__ == '__main__': + parser = argparse.ArgumentParser( + description='Convert ORB_SLAM2 output to OpenSfM') + parser.add_argument( + 'video', + help='the tracked video file') + parser.add_argument( + 'trajectory', + help='the trajectory file') + parser.add_argument( + 'config', + help='config file with camera calibration') + args = parser.parse_args() -with open('reconstruction.json', 'w') as fout: - json.dump([r], fout, indent=4) + r = { + 'cameras': {}, + 'shots': {} + } -extract_keyframes_from_video(args.video, r) + r['cameras']['slamcam'] = camera_from_config(args.video, args.config) + r['shots'] = shots_from_trajectory(args.trajectory) + + with open('reconstruction.json', 'w') as fout: + json.dump([r], fout, indent=4) + + extract_keyframes_from_video(args.video, r) From 0201ba44971e86ee9fe2995aa841bc15c0c9a010 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Thu, 4 Feb 2016 14:17:45 +0100 Subject: [PATCH 09/28] Convert from orb_slam to pmts --- modules/odm_slam/src/orb_slam_to_opensfm.py | 3 +- opendm/context.py | 4 ++ scripts/odm_slam.py | 55 ++++++++++++++++++--- 3 files changed, 54 insertions(+), 8 deletions(-) diff --git a/modules/odm_slam/src/orb_slam_to_opensfm.py b/modules/odm_slam/src/orb_slam_to_opensfm.py index aa6f5fe5..8b06cba3 100644 --- a/modules/odm_slam/src/orb_slam_to_opensfm.py +++ b/modules/odm_slam/src/orb_slam_to_opensfm.py @@ -128,7 +128,8 @@ if __name__ == '__main__': r = { 'cameras': {}, - 'shots': {} + 'shots': {}, + 'points': {}, } r['cameras']['slamcam'] = camera_from_config(args.video, args.config) diff --git a/opendm/context.py b/opendm/context.py index bf32e0d6..6faa73df 100644 --- a/opendm/context.py +++ b/opendm/context.py @@ -16,6 +16,9 @@ sys.path.append(pyopencv_path) # define opensfm path opensfm_path = os.path.join(superbuild_path, "src/opensfm") +# define orb_slam2 path +orb_slam2_path = os.path.join(superbuild_path, "src/orb_slam2") + # define pmvs path cmvs_path = os.path.join(superbuild_path, "install/bin/cmvs") cmvs_opts_path = os.path.join(superbuild_path, "install/bin/genOption") @@ -26,6 +29,7 @@ txt2las_path = os.path.join(superbuild_path, 'src/las-tools/bin') # define odm modules path odm_modules_path = os.path.join(root_path, "build/bin") +odm_modules_src_path = os.path.join(root_path, "modules") # Define supported image extensions supported_extensions = {'.jpg','.jpeg','.png'} diff --git a/scripts/odm_slam.py b/scripts/odm_slam.py index 5116ad5d..72e826fc 100644 --- a/scripts/odm_slam.py +++ b/scripts/odm_slam.py @@ -3,6 +3,7 @@ import os import ecto from opendm import log +from opendm import io from opendm import system from opendm import context @@ -33,12 +34,52 @@ class ODMSlamCell(ecto.Cell): system.mkdir_p(tree.opensfm) system.mkdir_p(tree.pmvs) - # run meshing binary - system.run( - '{}/odm_slam ' - 'SuperBuild/src/orb_slam2/Vocabulary/ORBvoc.txt ' - 'SuperBuild/src/orb_slam2/Examples/Monocular/TUM1.yaml ' - '{}'.format(context.odm_modules_path, video)) + vocabulary = os.path.join(context.orb_slam2_path, 'Vocabulary/ORBvoc.txt') + config_file = os.path.join(context.orb_slam2_path, 'Examples/Monocular/TUM1.yaml') + orb_slam_cmd = os.path.join(context.odm_modules_path, 'odm_slam') + + # run slam binary + system.run(' '.join([ + 'cd {} &&'.format(tree.opensfm), + orb_slam_cmd, + vocabulary, + config_file, + video, + ])) + + system.run(' '.join([ + 'cd {} &&'.format(tree.opensfm), + 'PYTHONPATH={}:{}'.format(context.pyopencv_path, context.opensfm_path), + 'python', + os.path.join(context.odm_modules_src_path, 'odm_slam/src/orb_slam_to_opensfm.py'), + video, + os.path.join(tree.opensfm, 'KeyFrameTrajectory.txt'), + config_file, + ])) + + + + ### check if reconstruction was exported to bundler before + + if not io.file_exists(tree.opensfm_bundle_list) or rerun_cell: + # convert back to bundler's format + system.run('PYTHONPATH=%s %s/bin/export_bundler %s' % + (context.pyopencv_path, context.opensfm_path, tree.opensfm)) + else: + log.ODM_WARNING('Found a valid Bundler file in: %s' % + (tree.opensfm_reconstruction)) + + + ### check if reconstruction was exported to pmvs before + + if not io.file_exists(tree.pmvs_visdat) or rerun_cell: + # run PMVS converter + system.run('PYTHONPATH=%s %s/bin/export_pmvs %s --output %s' % + (context.pyopencv_path, context.opensfm_path, tree.opensfm, tree.pmvs)) + else: + log.ODM_WARNING('Found a valid CMVS file in: %s' % tree.pmvs_visdat) log.ODM_INFO('Running OMD Slam Cell - Finished') - return ecto.OK if args['end_with'] != 'opensfm' else ecto.QUIT + return ecto.OK if args['end_with'] != 'odm_slam' else ecto.QUIT + + From 005830b53ceac9c27f0721271466110fc04dfc35 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Thu, 4 Feb 2016 14:24:04 +0100 Subject: [PATCH 10/28] Pass slam config as a command line argument --- opendm/config.py | 4 ++++ scripts/odm_slam.py | 19 +++++++++---------- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/opendm/config.py b/opendm/config.py index 60921204..a25c9705 100644 --- a/opendm/config.py +++ b/opendm/config.py @@ -37,6 +37,10 @@ parser.add_argument('--video', metavar='', help='Path to the video file to process') +parser.add_argument('--slam-config', + metavar='', + help='Path to config file for orb-slam') + parser.add_argument('--force-focal', metavar='', type=float, diff --git a/scripts/odm_slam.py b/scripts/odm_slam.py index 72e826fc..e2988240 100644 --- a/scripts/odm_slam.py +++ b/scripts/odm_slam.py @@ -25,6 +25,7 @@ class ODMSlamCell(ecto.Cell): tree = self.inputs.tree args = self.inputs.args video = os.path.join(tree.root_path, args['video']) + slam_config = os.path.join(tree.root_path, args['slam_config']) if not video: log.ODM_ERROR('No video provided') @@ -35,17 +36,16 @@ class ODMSlamCell(ecto.Cell): system.mkdir_p(tree.pmvs) vocabulary = os.path.join(context.orb_slam2_path, 'Vocabulary/ORBvoc.txt') - config_file = os.path.join(context.orb_slam2_path, 'Examples/Monocular/TUM1.yaml') orb_slam_cmd = os.path.join(context.odm_modules_path, 'odm_slam') # run slam binary - system.run(' '.join([ - 'cd {} &&'.format(tree.opensfm), - orb_slam_cmd, - vocabulary, - config_file, - video, - ])) + # system.run(' '.join([ + # 'cd {} &&'.format(tree.opensfm), + # orb_slam_cmd, + # vocabulary, + # slam_config, + # video, + # ])) system.run(' '.join([ 'cd {} &&'.format(tree.opensfm), @@ -54,11 +54,10 @@ class ODMSlamCell(ecto.Cell): os.path.join(context.odm_modules_src_path, 'odm_slam/src/orb_slam_to_opensfm.py'), video, os.path.join(tree.opensfm, 'KeyFrameTrajectory.txt'), - config_file, + slam_config, ])) - ### check if reconstruction was exported to bundler before if not io.file_exists(tree.opensfm_bundle_list) or rerun_cell: From cafc98917cb6bd06cd3e3cfcc218f425149aeb60 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Thu, 4 Feb 2016 14:35:15 +0100 Subject: [PATCH 11/28] Rerun slam only when required --- modules/odm_slam/src/OdmSlam.cpp | 4 --- opendm/config.py | 2 +- scripts/odm_slam.py | 54 +++++++++++++++++++------------- 3 files changed, 34 insertions(+), 26 deletions(-) diff --git a/modules/odm_slam/src/OdmSlam.cpp b/modules/odm_slam/src/OdmSlam.cpp index dce6df3f..c48e9f2a 100644 --- a/modules/odm_slam/src/OdmSlam.cpp +++ b/modules/odm_slam/src/OdmSlam.cpp @@ -30,11 +30,7 @@ int main(int argc, char **argv) { cap >> im; if(im.empty()) break; - // Save double timestamp = ni * T; - keyframe_timestamps << ni << " " << timestamp << std::endl; - - SLAM.TrackMonocular(im, timestamp); diff --git a/opendm/config.py b/opendm/config.py index a25c9705..02c40855 100644 --- a/opendm/config.py +++ b/opendm/config.py @@ -1,7 +1,7 @@ import argparse # parse arguments -processopts = ['resize', 'opensfm', 'cmvs', 'pmvs', +processopts = ['resize', 'opensfm', 'slam', 'cmvs', 'pmvs', 'odm_meshing', 'odm_texturing', 'odm_georeferencing', 'odm_orthophoto'] diff --git a/scripts/odm_slam.py b/scripts/odm_slam.py index e2988240..2f1feafb 100644 --- a/scripts/odm_slam.py +++ b/scripts/odm_slam.py @@ -37,29 +37,42 @@ class ODMSlamCell(ecto.Cell): vocabulary = os.path.join(context.orb_slam2_path, 'Vocabulary/ORBvoc.txt') orb_slam_cmd = os.path.join(context.odm_modules_path, 'odm_slam') + trajectory = os.path.join(tree.opensfm, 'KeyFrameTrajectory.txt') - # run slam binary - # system.run(' '.join([ - # 'cd {} &&'.format(tree.opensfm), - # orb_slam_cmd, - # vocabulary, - # slam_config, - # video, - # ])) + # check if we rerun cell or not + rerun_cell = args['rerun'] == 'slam' - system.run(' '.join([ - 'cd {} &&'.format(tree.opensfm), - 'PYTHONPATH={}:{}'.format(context.pyopencv_path, context.opensfm_path), - 'python', - os.path.join(context.odm_modules_src_path, 'odm_slam/src/orb_slam_to_opensfm.py'), - video, - os.path.join(tree.opensfm, 'KeyFrameTrajectory.txt'), - slam_config, - ])) + # check if slam was run before + if not io.file_exists(trajectory) or rerun_cell: + # run slam binary + system.run(' '.join([ + 'cd {} &&'.format(tree.opensfm), + orb_slam_cmd, + vocabulary, + slam_config, + video, + ])) + else: + log.ODM_WARNING('Found a valid slam trajectory in: {}'.format( + trajectory)) + # check if trajectory was exported to opensfm before + if not io.file_exists(tree.opensfm_reconstruction) or rerun_cell: + # convert slam to opensfm + system.run(' '.join([ + 'cd {} &&'.format(tree.opensfm), + 'PYTHONPATH={}:{}'.format(context.pyopencv_path, context.opensfm_path), + 'python', + os.path.join(context.odm_modules_src_path, 'odm_slam/src/orb_slam_to_opensfm.py'), + video, + trajectory, + slam_config, + ])) + else: + log.ODM_WARNING('Found a valid OpenSfm file in: {}'.format( + tree.opensfm_reconstruction)) - ### check if reconstruction was exported to bundler before - + # check if reconstruction was exported to bundler before if not io.file_exists(tree.opensfm_bundle_list) or rerun_cell: # convert back to bundler's format system.run('PYTHONPATH=%s %s/bin/export_bundler %s' % @@ -69,8 +82,7 @@ class ODMSlamCell(ecto.Cell): (tree.opensfm_reconstruction)) - ### check if reconstruction was exported to pmvs before - + # check if reconstruction was exported to pmvs before if not io.file_exists(tree.pmvs_visdat) or rerun_cell: # run PMVS converter system.run('PYTHONPATH=%s %s/bin/export_pmvs %s --output %s' % From 62323382104ba7778845c74fe8afc4df88798241 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Fri, 5 Feb 2016 01:02:55 +0100 Subject: [PATCH 12/28] Export orb slam map points --- SuperBuild/cmake/External-ORB_SLAM2.cmake | 3 +- modules/odm_slam/src/OdmSlam.cpp | 54 +++++++++++++++++++-- modules/odm_slam/src/orb_slam_to_opensfm.py | 54 ++++++++++++++++++++- scripts/odm_slam.py | 2 + 4 files changed, 105 insertions(+), 8 deletions(-) diff --git a/SuperBuild/cmake/External-ORB_SLAM2.cmake b/SuperBuild/cmake/External-ORB_SLAM2.cmake index d7f11ae2..9e704784 100644 --- a/SuperBuild/cmake/External-ORB_SLAM2.cmake +++ b/SuperBuild/cmake/External-ORB_SLAM2.cmake @@ -8,8 +8,7 @@ ExternalProject_Add(${_proj_name} STAMP_DIR ${_SB_BINARY_DIR}/stamp #--Download step-------------- DOWNLOAD_DIR ${SB_DOWNLOAD_DIR} - URL https://github.com/raulmur/ORB_SLAM2/archive/2fc9730d9716c36b25ea1b0eba3bbd094e20f0d5.zip - URL_MD5 629f19b6d424e676ce82fa1c3a0ba43b + URL https://github.com/paulinus/ORB_SLAM2/archive/7c11f186a53a75560cd17352d327b0bc127a82de.zip #--Update/Patch step---------- UPDATE_COMMAND "" #--Configure step------------- diff --git a/modules/odm_slam/src/OdmSlam.cpp b/modules/odm_slam/src/OdmSlam.cpp index c48e9f2a..fd6b4b68 100644 --- a/modules/odm_slam/src/OdmSlam.cpp +++ b/modules/odm_slam/src/OdmSlam.cpp @@ -3,6 +3,52 @@ #include #include +#include + + +void SaveKeyFrameTrajectory(ORB_SLAM2::Map *map, const string &filename, const string &tracksfile) { + std::cout << std::endl << "Saving keyframe trajectory to " << filename << " ..." << std::endl; + + vector vpKFs = map->GetAllKeyFrames(); + sort(vpKFs.begin(), vpKFs.end(), ORB_SLAM2::KeyFrame::lId); + + std::ofstream f; + f.open(filename.c_str()); + f << fixed; + + std::ofstream fpoints; + fpoints.open(tracksfile.c_str()); + fpoints << fixed; + + for(size_t i = 0; i < vpKFs.size(); i++) { + ORB_SLAM2::KeyFrame* pKF = vpKFs[i]; + + if(pKF->isBad()) + continue; + + cv::Mat R = pKF->GetRotation().t(); + vector q = ORB_SLAM2::Converter::toQuaternion(R); + cv::Mat t = pKF->GetCameraCenter(); + f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at(0) << " " << t.at(1) << " " << t.at(2) + << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << std::endl; + + for (auto point : pKF->GetMapPoints()) { + auto coords = point->GetWorldPos(); + fpoints << setprecision(6) + << pKF->mTimeStamp + << " " << point->mnId + << setprecision(7) + << " " << coords.at(0, 0) + << " " << coords.at(1, 0) + << " " << coords.at(2, 0) + << std::endl; + } + } + + f.close(); + fpoints.close(); + std::cout << std::endl << "trajectory saved!" << std::endl; +} int main(int argc, char **argv) { @@ -19,9 +65,9 @@ int main(int argc, char **argv) { return -1; } - ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, false); + ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true); - cout << "Start processing video ..." << endl; + std::cout << "Start processing video ..." << std::endl; double T = 0.1; // Seconds between frames cv::Mat im; @@ -34,11 +80,11 @@ int main(int argc, char **argv) { SLAM.TrackMonocular(im, timestamp); - usleep(int(T * 1e6)); + //usleep(int(T * 1e6)); } SLAM.Shutdown(); - SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + SaveKeyFrameTrajectory(SLAM.GetMap(), "KeyFrameTrajectory.txt", "MapPoints.txt"); return 0; } diff --git a/modules/odm_slam/src/orb_slam_to_opensfm.py b/modules/odm_slam/src/orb_slam_to_opensfm.py index 8b06cba3..bdbff965 100644 --- a/modules/odm_slam/src/orb_slam_to_opensfm.py +++ b/modules/odm_slam/src/orb_slam_to_opensfm.py @@ -9,6 +9,9 @@ from opensfm import transformations as tf from opensfm.io import mkdir_p +SCALE = 50 + + def parse_orb_slam2_config_file(filename): ''' Parse ORB_SLAM2 config file. @@ -51,6 +54,12 @@ def camera_from_config(video_filename, config_filename): } +def shot_id_from_timestamp(timestamp): + T = 0.1 # TODO(pau) get this from config + i = int(round(timestamp / T)) + return 'frame{0:06d}.png'.format(i) + + def shots_from_trajectory(trajectory_filename): ''' Create opensfm shots from an orb_slam2/TUM trajectory @@ -65,17 +74,51 @@ def shots_from_trajectory(trajectory_filename): c = np.array(a[1:4]) q = np.array(a[4:8]) R = tf.quaternion_matrix([q[3], q[0], q[1], q[2]])[:3, :3].T - t = -R.dot(c) * 50 + t = -R.dot(c) * SCALE shot = { 'camera': 'slamcam', 'rotation': list(cv2.Rodrigues(R)[0].flat), 'translation': list(t.flat), 'created_at': timestamp, } - shots['frame{0:012d}.png'.format(int(timestamp))] = shot + shots[shot_id_from_timestamp(timestamp)] = shot return shots +def points_from_map_points(filename): + points = {} + with open(filename) as fin: + lines = fin.readlines() + + for line in lines: + words = line.split() + point_id = words[1] + coords = map(float, words[2:5]) + coords = [SCALE * i for i in coords] + points[point_id] = { + 'coordinates': coords, + 'color': [100, 0, 200] + } + + return points + + +def tracks_from_map_points(filename): + tracks = [] + with open(filename) as fin: + lines = fin.readlines() + + for line in lines: + words = line.split() + timestamp = float(words[0]) + shot_id = shot_id_from_timestamp(timestamp) + point_id = words[1] + row = [shot_id, point_id, point_id, '0', '0', '0', '0', '0'] + tracks.append('\t'.join(row)) + + return '\n'.join(tracks) + + def get_video_size(video): cap = cv2.VideoCapture(video) width = int(cap.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH)) @@ -121,6 +164,9 @@ if __name__ == '__main__': parser.add_argument( 'trajectory', help='the trajectory file') + parser.add_argument( + 'points', + help='the map points file') parser.add_argument( 'config', help='config file with camera calibration') @@ -134,8 +180,12 @@ if __name__ == '__main__': r['cameras']['slamcam'] = camera_from_config(args.video, args.config) r['shots'] = shots_from_trajectory(args.trajectory) + r['points'] = points_from_map_points(args.points) + tracks = tracks_from_map_points(args.points) with open('reconstruction.json', 'w') as fout: json.dump([r], fout, indent=4) + with open('tracks.csv', 'w') as fout: + fout.write(tracks) extract_keyframes_from_video(args.video, r) diff --git a/scripts/odm_slam.py b/scripts/odm_slam.py index 2f1feafb..aaecaf0e 100644 --- a/scripts/odm_slam.py +++ b/scripts/odm_slam.py @@ -38,6 +38,7 @@ class ODMSlamCell(ecto.Cell): vocabulary = os.path.join(context.orb_slam2_path, 'Vocabulary/ORBvoc.txt') orb_slam_cmd = os.path.join(context.odm_modules_path, 'odm_slam') trajectory = os.path.join(tree.opensfm, 'KeyFrameTrajectory.txt') + map_points = os.path.join(tree.opensfm, 'MapPoints.txt') # check if we rerun cell or not rerun_cell = args['rerun'] == 'slam' @@ -66,6 +67,7 @@ class ODMSlamCell(ecto.Cell): os.path.join(context.odm_modules_src_path, 'odm_slam/src/orb_slam_to_opensfm.py'), video, trajectory, + map_points, slam_config, ])) else: From 0848ecd545d7cd7f339acf5b964ab84cc807aecc Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Tue, 1 Mar 2016 12:01:52 +0100 Subject: [PATCH 13/28] Formatting --- scripts/odm_slam.py | 41 ++++++++++++++++++++++++++--------------- 1 file changed, 26 insertions(+), 15 deletions(-) diff --git a/scripts/odm_slam.py b/scripts/odm_slam.py index aaecaf0e..2a45709a 100644 --- a/scripts/odm_slam.py +++ b/scripts/odm_slam.py @@ -1,3 +1,5 @@ +"""Cell to run odm_slam.""" + import os import ecto @@ -9,16 +11,20 @@ from opendm import context class ODMSlamCell(ecto.Cell): + """Run odm_slam on a video and export to opensfm format.""" + def declare_params(self, params): + """Cell parameters.""" pass def declare_io(self, params, inputs, outputs): + """Cell inputs and outputs.""" inputs.declare("tree", "Struct with paths", []) inputs.declare("args", "The application arguments.", {}) outputs.declare("reconstruction", "list of ODMReconstructions", []) def process(self, inputs, outputs): - + """Run the cell.""" log.ODM_INFO('Running OMD Slam Cell') # get inputs @@ -35,7 +41,8 @@ class ODMSlamCell(ecto.Cell): system.mkdir_p(tree.opensfm) system.mkdir_p(tree.pmvs) - vocabulary = os.path.join(context.orb_slam2_path, 'Vocabulary/ORBvoc.txt') + vocabulary = os.path.join(context.orb_slam2_path, + 'Vocabulary/ORBvoc.txt') orb_slam_cmd = os.path.join(context.odm_modules_path, 'odm_slam') trajectory = os.path.join(tree.opensfm, 'KeyFrameTrajectory.txt') map_points = os.path.join(tree.opensfm, 'MapPoints.txt') @@ -62,37 +69,41 @@ class ODMSlamCell(ecto.Cell): # convert slam to opensfm system.run(' '.join([ 'cd {} &&'.format(tree.opensfm), - 'PYTHONPATH={}:{}'.format(context.pyopencv_path, context.opensfm_path), + 'PYTHONPATH={}:{}'.format(context.pyopencv_path, + context.opensfm_path), 'python', - os.path.join(context.odm_modules_src_path, 'odm_slam/src/orb_slam_to_opensfm.py'), + os.path.join(context.odm_modules_src_path, + 'odm_slam/src/orb_slam_to_opensfm.py'), video, trajectory, map_points, slam_config, ])) else: - log.ODM_WARNING('Found a valid OpenSfm file in: {}'.format( + log.ODM_WARNING('Found a valid OpenSfM file in: {}'.format( tree.opensfm_reconstruction)) # check if reconstruction was exported to bundler before if not io.file_exists(tree.opensfm_bundle_list) or rerun_cell: # convert back to bundler's format - system.run('PYTHONPATH=%s %s/bin/export_bundler %s' % - (context.pyopencv_path, context.opensfm_path, tree.opensfm)) + system.run( + 'PYTHONPATH={} {}/bin/export_bundler {}'.format( + context.pyopencv_path, context.opensfm_path, tree.opensfm)) else: - log.ODM_WARNING('Found a valid Bundler file in: %s' % - (tree.opensfm_reconstruction)) - + log.ODM_WARNING( + 'Found a valid Bundler file in: {}'.format( + tree.opensfm_reconstruction)) # check if reconstruction was exported to pmvs before if not io.file_exists(tree.pmvs_visdat) or rerun_cell: # run PMVS converter - system.run('PYTHONPATH=%s %s/bin/export_pmvs %s --output %s' % - (context.pyopencv_path, context.opensfm_path, tree.opensfm, tree.pmvs)) + system.run( + 'PYTHONPATH={} {}/bin/export_pmvs {} --output {}'.format( + context.pyopencv_path, context.opensfm_path, tree.opensfm, + tree.pmvs)) else: - log.ODM_WARNING('Found a valid CMVS file in: %s' % tree.pmvs_visdat) + log.ODM_WARNING('Found a valid CMVS file in: {}'.format( + tree.pmvs_visdat)) log.ODM_INFO('Running OMD Slam Cell - Finished') return ecto.OK if args['end_with'] != 'odm_slam' else ecto.QUIT - - From c26e9751f6c89672a6cab926100d1353690af616 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Tue, 1 Mar 2016 12:02:11 +0100 Subject: [PATCH 14/28] Update lib Pangolin --- SuperBuild/cmake/External-Pangolin.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/SuperBuild/cmake/External-Pangolin.cmake b/SuperBuild/cmake/External-Pangolin.cmake index 9fa02345..f328c89b 100644 --- a/SuperBuild/cmake/External-Pangolin.cmake +++ b/SuperBuild/cmake/External-Pangolin.cmake @@ -7,8 +7,8 @@ ExternalProject_Add(${_proj_name} STAMP_DIR ${_SB_BINARY_DIR}/stamp #--Download step-------------- DOWNLOAD_DIR ${SB_DOWNLOAD_DIR} - URL https://github.com/paulinus/Pangolin/archive/952c62bd6adbbc8bf58c5941fbb353ce7ff9e022.zip - URL_MD5 1b8a3713a1b9da699187cf1e8dc60eab + URL https://github.com/paulinus/Pangolin/archive/b7c66570b336e012bf3124e2a7411d417a1d35f7.zip + URL_MD5 9b7938d1045d26b27a637b663e647aef #--Update/Patch step---------- UPDATE_COMMAND "" #--Configure step------------- From 0e614ee46a88e2d40f5b1c3d74166f9391fe528a Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Tue, 1 Mar 2016 12:03:02 +0100 Subject: [PATCH 15/28] Retry fetching images from video on failures --- modules/odm_slam/src/OdmSlam.cpp | 12 ++++++++++-- modules/odm_slam/src/orb_slam_to_opensfm.py | 21 +++++++++++++-------- 2 files changed, 23 insertions(+), 10 deletions(-) diff --git a/modules/odm_slam/src/OdmSlam.cpp b/modules/odm_slam/src/OdmSlam.cpp index fd6b4b68..1fe7505d 100644 --- a/modules/odm_slam/src/OdmSlam.cpp +++ b/modules/odm_slam/src/OdmSlam.cpp @@ -67,14 +67,22 @@ int main(int argc, char **argv) { ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true); + usleep(10 * 1e6); + std::cout << "Start processing video ..." << std::endl; double T = 0.1; // Seconds between frames cv::Mat im; + int num_frames = cap.get(CV_CAP_PROP_FRAME_COUNT); for(int ni = 0;; ++ni){ + std::cout << "processing frame " << ni << "/" << num_frames << std::endl; // Get frame - cap >> im; - if(im.empty()) break; + bool res = false; + for (int trial = 0; !res && trial < 20; ++trial) { + std::cout << "trial " << trial << std::endl; + res = cap.read(im); + } + if(!res) break; double timestamp = ni * T; diff --git a/modules/odm_slam/src/orb_slam_to_opensfm.py b/modules/odm_slam/src/orb_slam_to_opensfm.py index bdbff965..fae1feda 100644 --- a/modules/odm_slam/src/orb_slam_to_opensfm.py +++ b/modules/odm_slam/src/orb_slam_to_opensfm.py @@ -142,15 +142,20 @@ def extract_keyframes_from_video(video, reconstruction): shot = reconstruction['shots'][shot_id] timestamp = shot['created_at'] keyframe_idx = int(round(timestamp / T)) - while video_idx < keyframe_idx: - ret, frame = cap.read() + + while video_idx <= keyframe_idx: + for i in range(20): + ret, frame = cap.read() + if ret: + break + else: + print 'retrying' + if not ret: + raise RuntimeError( + 'Cound not find keyframe {} in video'.format(shot_id)) + if video_idx == keyframe_idx: + cv2.imwrite(os.path.join(image_path, shot_id), frame) video_idx += 1 - if video_idx == keyframe_idx: - print 'saving', shot_id - cv2.imwrite(os.path.join(image_path, shot_id), frame) - else: - raise RuntimeError( - 'Cound not find keyframe {} in video'.format(shot_id)) cap.release() From 109eb800f14d9013d3e20d02aa8713014240fce4 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Thu, 3 Mar 2016 12:58:25 +0100 Subject: [PATCH 16/28] Radially undistort images before odm_texturing --- modules/odm_slam/src/undistort_radial.py | 53 ++++++++++++++++++++++++ opendm/types.py | 2 + scripts/odm_slam.py | 2 + scripts/odm_texturing.py | 28 ++++++++++++- 4 files changed, 83 insertions(+), 2 deletions(-) create mode 100644 modules/odm_slam/src/undistort_radial.py diff --git a/modules/odm_slam/src/undistort_radial.py b/modules/odm_slam/src/undistort_radial.py new file mode 100644 index 00000000..db40afa7 --- /dev/null +++ b/modules/odm_slam/src/undistort_radial.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python + +import argparse +import os + +import cv2 +import numpy as np + +import opensfm.dataset as dataset +import opensfm.io as io + + +def opencv_calibration_matrix(width, height, focal): + '''Calibration matrix as used by OpenCV and PMVS + ''' + f = focal * max(width, height) + return np.matrix([[f, 0, 0.5 * (width - 1)], + [0, f, 0.5 * (height - 1)], + [0, 0, 1.0]]) + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Undistort images') + parser.add_argument('dataset', help='path to the dataset to be processed') + parser.add_argument('--output', help='output folder for the undistorted images') + args = parser.parse_args() + + data = dataset.DataSet(args.dataset) + if args.output: + output_path = args.output + else: + output_path = os.path.join(data.data_path, 'undistorted') + + print "Undistorting images from dataset [%s] to dir [%s]" % (data.data_path, output_path) + + io.mkdir_p(output_path) + + reconstructions = data.load_reconstruction() + for h, reconstruction in enumerate(reconstructions): + print "undistorting reconstruction", h + for image in reconstruction['shots']: + print "undistorting image", image + shot = reconstruction["shots"][image] + + original_image = data.image_as_array(image)[:,:,::-1] + camera = reconstruction['cameras'][shot['camera']] + original_h, original_w = original_image.shape[:2] + K = opencv_calibration_matrix(original_w, original_h, camera['focal']) + k1 = camera["k1"] + k2 = camera["k2"] + undistorted_image = cv2.undistort(original_image, K, np.array([k1, k2, 0, 0])) + + new_image_path = os.path.join(output_path, image.split('/')[-1]) + cv2.imwrite(new_image_path, undistorted_image) diff --git a/opendm/types.py b/opendm/types.py index 2a9682d5..d120a877 100644 --- a/opendm/types.py +++ b/opendm/types.py @@ -300,6 +300,8 @@ class ODM_Tree(object): self.odm_meshing_log = io.join_paths(self.odm_meshing, 'odm_meshing_log.txt') # odm_texturing + self.odm_texturing_undistorted_image_path = io.join_paths( + self.odm_texturing, 'undistorted') self.odm_textured_model_obj = io.join_paths( self.odm_texturing, 'odm_textured_model.obj') self.odm_textured_model_mtl = io.join_paths( diff --git a/scripts/odm_slam.py b/scripts/odm_slam.py index 2a45709a..dbd8fb2d 100644 --- a/scripts/odm_slam.py +++ b/scripts/odm_slam.py @@ -79,6 +79,8 @@ class ODMSlamCell(ecto.Cell): map_points, slam_config, ])) + # link opensfm images to resized images + os.symlink(tree.opensfm + '/images', tree.dataset_resize) else: log.ODM_WARNING('Found a valid OpenSfM file in: {}'.format( tree.opensfm_reconstruction)) diff --git a/scripts/odm_texturing.py b/scripts/odm_texturing.py index 89b72650..61fad467 100644 --- a/scripts/odm_texturing.py +++ b/scripts/odm_texturing.py @@ -1,3 +1,5 @@ +import os + import ecto from opendm import log @@ -35,6 +37,28 @@ class ODMTexturingCell(ecto.Cell): rerun_cell = args['rerun'] is not None \ and args['rerun'] == 'odm_texturing' + # Undistort radial distortion + if not os.path.isdir(tree.odm_texturing_undistorted_image_path) or rerun_cell: + system.run(' '.join([ + 'cd {} &&'.format(tree.opensfm), + 'PYTHONPATH={}:{}'.format(context.pyopencv_path, + context.opensfm_path), + 'python', + os.path.join(context.odm_modules_src_path, + 'odm_slam/src/undistort_radial.py'), + '--output', + tree.odm_texturing_undistorted_image_path, + tree.opensfm, + ])) + + system.run( + 'PYTHONPATH=%s %s/bin/export_bundler %s' % + (context.pyopencv_path, context.opensfm_path, tree.opensfm)) + else: + log.ODM_WARNING( + 'Found a valid Bundler file in: %s' % + (tree.opensfm_reconstruction)) + if not io.file_exists(tree.odm_textured_model_obj) or rerun_cell: log.ODM_DEBUG('Writting ODM Textured file in: %s' \ % tree.odm_textured_model_obj) @@ -44,7 +68,7 @@ class ODMTexturingCell(ecto.Cell): 'bin': context.odm_modules_path, 'out_dir': tree.odm_texturing, 'bundle': tree.opensfm_bundle, - 'imgs_path': tree.dataset_resize, + 'imgs_path': tree.odm_texturing_undistorted_image_path, 'imgs_list': tree.opensfm_bundle_list, 'model': tree.odm_mesh, 'log': tree.odm_texuring_log, @@ -64,4 +88,4 @@ class ODMTexturingCell(ecto.Cell): % tree.odm_textured_model_obj) log.ODM_INFO('Running OMD Texturing Cell - Finished') - return ecto.OK if args['end_with'] != 'odm_texturing' else ecto.QUIT \ No newline at end of file + return ecto.OK if args['end_with'] != 'odm_texturing' else ecto.QUIT From 4ae28497398c69182133314bda618e4470f94cfa Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Wed, 18 May 2016 14:23:39 +0200 Subject: [PATCH 17/28] Fix connections --- opendm/config.py | 2 ++ scripts/odm_app.py | 5 +++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/opendm/config.py b/opendm/config.py index 37ab5ff9..5327b105 100644 --- a/opendm/config.py +++ b/opendm/config.py @@ -13,6 +13,8 @@ class RerunFrom(argparse.Action): parser = argparse.ArgumentParser(description='OpenDroneMap') + + def config(): parser.add_argument('--project-path', metavar='', diff --git a/scripts/odm_app.py b/scripts/odm_app.py index 70b0eaf9..bab666bd 100644 --- a/scripts/odm_app.py +++ b/scripts/odm_app.py @@ -66,6 +66,7 @@ class ODMApp(ecto.BlackBox): gcp_file=p.args.odm_georeferencing_gcpFile, use_gcp=p.args.odm_georeferencing_useGcp), 'orthophoto': ODMOrthoPhotoCell(resolution=p.args.odm_orthophoto_resolution) + } return cells @@ -82,7 +83,7 @@ class ODMApp(ecto.BlackBox): b.write('ODM Benchmarking file created %s\nNumber of Cores: %s\n\n' % (system.now(), context.num_cores)) def connections(self, _p): - run_slam = _p.args.get('video') is not None + run_slam = _p.args.video # define initial task # TODO: What is this? @@ -103,7 +104,7 @@ class ODMApp(ecto.BlackBox): self.slam['reconstruction'] >> self.cmvs['reconstruction']] else: # load the dataset - connections = [self.tree[:] >> self.dataset['tree']] + connections += [self.tree[:] >> self.dataset['tree']] # run resize cell connections += [self.tree[:] >> self.resize['tree'], From 42429b3d327c2957b09e689065df0cb705c11c9d Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Wed, 18 May 2016 15:49:11 +0200 Subject: [PATCH 18/28] Use args as an object instead of a dict --- scripts/odm_slam.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/scripts/odm_slam.py b/scripts/odm_slam.py index dbd8fb2d..b905f4a5 100644 --- a/scripts/odm_slam.py +++ b/scripts/odm_slam.py @@ -30,8 +30,8 @@ class ODMSlamCell(ecto.Cell): # get inputs tree = self.inputs.tree args = self.inputs.args - video = os.path.join(tree.root_path, args['video']) - slam_config = os.path.join(tree.root_path, args['slam_config']) + video = os.path.join(tree.root_path, args.video) + slam_config = os.path.join(tree.root_path, args.slam_config) if not video: log.ODM_ERROR('No video provided') @@ -48,7 +48,7 @@ class ODMSlamCell(ecto.Cell): map_points = os.path.join(tree.opensfm, 'MapPoints.txt') # check if we rerun cell or not - rerun_cell = args['rerun'] == 'slam' + rerun_cell = args.rerun == 'slam' # check if slam was run before if not io.file_exists(trajectory) or rerun_cell: @@ -108,4 +108,4 @@ class ODMSlamCell(ecto.Cell): tree.pmvs_visdat)) log.ODM_INFO('Running OMD Slam Cell - Finished') - return ecto.OK if args['end_with'] != 'odm_slam' else ecto.QUIT + return ecto.OK if args.end_with != 'odm_slam' else ecto.QUIT From a205c38a5b363f8be8b7210c60086829d1a1aaf7 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Thu, 19 May 2016 16:51:08 +0200 Subject: [PATCH 19/28] Add calibration from video script --- modules/odm_slam/src/calibrate_video.py | 134 ++++++++++++++++++++++++ 1 file changed, 134 insertions(+) create mode 100644 modules/odm_slam/src/calibrate_video.py diff --git a/modules/odm_slam/src/calibrate_video.py b/modules/odm_slam/src/calibrate_video.py new file mode 100644 index 00000000..02ce31c7 --- /dev/null +++ b/modules/odm_slam/src/calibrate_video.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python + +import argparse +import sys + +import numpy as np +import cv2 + + +class Calibrator: + """Camera calibration using a chessboard pattern.""" + + def __init__(self, pattern_width, pattern_height, motion_threshold=0.05): + """Init the calibrator. + + The parameter motion_threshold determines the minimal motion required + to add a new frame to the calibration data, as a ratio of image width. + """ + self.pattern_size = (pattern_width, pattern_height) + self.motion_threshold = motion_threshold + self.pattern_points = np.array([ + (i, j, 0.0) + for j in range(pattern_height) + for i in range(pattern_width) + ], dtype=np.float32) + self.object_points = [] + self.image_points = [] + + def process_image(self, image, window_name): + """Find corners of an image and store them internally.""" + if len(image.shape) == 3: + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + else: + gray = image + + h, w = gray.shape + self.image_size = (w, h) + + found, corners = cv2.findChessboardCorners(gray, self.pattern_size) + + if found: + term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1) + cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), term) + self._add_points(corners.reshape(-1, 2)) + + if window_name: + cv2.drawChessboardCorners(image, self.pattern_size, corners, found) + cv2.imshow(window_name, image) + + return found + + def calibrate(self): + """Run calibration using points extracted by process_image.""" + rms, camera_matrix, dist_coefs, rvecs, tvecs = cv2.calibrateCamera( + self.object_points, self.image_points, self.image_size, None, None) + return rms, camera_matrix, dist_coefs + + def _add_points(self, image_points): + if self.image_points: + delta = np.fabs(image_points - self.image_points[-1]).max() + should_add = (delta > self.image_size[0] * self.motion_threshold) + else: + should_add = True + + if should_add: + self.image_points.append(image_points) + self.object_points.append(self.pattern_points) + + +def video_frames(filename): + """Yield frames in a video.""" + cap = cv2.VideoCapture(args.video) + while True: + ret, frame = cap.read() + if ret: + yield frame + else: + break + cap.release() + + +def parse_arguments(): + parser = argparse.ArgumentParser( + description="Camera calibration from video of a chessboard.") + parser.add_argument( + 'video', + help="video of the checkerboard") + parser.add_argument( + '--output', + default='calibration', + help="base name for the output files") + parser.add_argument( + '--size', + default='8x6', + help="size of the chessboard") + parser.add_argument( + '--visual', + action='store_true', + help="display images while calibrating") + return parser.parse_args() + + +if __name__ == '__main__': + args = parse_arguments() + + pattern_size = [int(i) for i in args.size.split('x')] + calibrator = Calibrator(pattern_size[0], pattern_size[1]) + + window_name = None + if args.visual: + window_name = 'Chessboard detection' + cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) + + print "kept\tcurrent\tchessboard found" + + for i, frame in enumerate(video_frames(args.video)): + found = calibrator.process_image(frame, window_name) + + print "{}\t{}\t{} \r".format( + len(calibrator.image_points), i, found), + sys.stdout.flush() + + if args.visual: + if cv2.waitKey(1) & 0xFF == ord('q'): + break + print + + cv2.destroyAllWindows() + + rms, camera_matrix, dist_coefs = calibrator.calibrate() + + print "RMS:", rms + print "camera matrix:\n", camera_matrix + print "distortion coefficients: ", dist_coefs.ravel() From aa54fd6c60c55cc45f907249ad120f91d3097f82 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Thu, 19 May 2016 17:25:46 +0200 Subject: [PATCH 20/28] Print calibration parameters in ORB_SLAM2 format --- modules/odm_slam/src/calibrate_video.py | 26 +++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/modules/odm_slam/src/calibrate_video.py b/modules/odm_slam/src/calibrate_video.py index 02ce31c7..59ae02aa 100644 --- a/modules/odm_slam/src/calibrate_video.py +++ b/modules/odm_slam/src/calibrate_video.py @@ -53,7 +53,7 @@ class Calibrator: """Run calibration using points extracted by process_image.""" rms, camera_matrix, dist_coefs, rvecs, tvecs = cv2.calibrateCamera( self.object_points, self.image_points, self.image_size, None, None) - return rms, camera_matrix, dist_coefs + return rms, camera_matrix, dist_coefs.ravel() def _add_points(self, image_points): if self.image_points: @@ -79,6 +79,24 @@ def video_frames(filename): cap.release() +def orb_slam_calibration_config(camera_matrix, dist_coefs): + """String with calibration parameters in orb_slam config format.""" + lines = [ + "# Camera calibration and distortion parameters (OpenCV)", + "Camera.fx: {}".format(camera_matrix[0, 0]), + "Camera.fy: {}".format(camera_matrix[1, 1]), + "Camera.cx: {}".format(camera_matrix[0, 2]), + "Camera.cy: {}".format(camera_matrix[1, 2]), + "", + "Camera.k1: {}".format(dist_coefs[0]), + "Camera.k2: {}".format(dist_coefs[1]), + "Camera.p1: {}".format(dist_coefs[2]), + "Camera.p2: {}".format(dist_coefs[3]), + "Camera.k3: {}".format(dist_coefs[4]), + ] + return "\n".join(lines) + + def parse_arguments(): parser = argparse.ArgumentParser( description="Camera calibration from video of a chessboard.") @@ -123,12 +141,12 @@ if __name__ == '__main__': if args.visual: if cv2.waitKey(1) & 0xFF == ord('q'): break - print cv2.destroyAllWindows() rms, camera_matrix, dist_coefs = calibrator.calibrate() + print print "RMS:", rms - print "camera matrix:\n", camera_matrix - print "distortion coefficients: ", dist_coefs.ravel() + print + print orb_slam_calibration_config(camera_matrix, dist_coefs) From d38ccab000a5fe946e69bc40669c9419958813c8 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Thu, 16 Jun 2016 10:28:12 +0200 Subject: [PATCH 21/28] Make SLAM build optional --- CMakeLists.txt | 4 +++- SuperBuild/CMakeLists.txt | 10 ++++++++-- SuperBuild/cmake/External-OpenCV.cmake | 4 ++-- modules/CMakeLists.txt | 4 +++- 4 files changed, 16 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4ca45386..a8a441d8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,5 +12,7 @@ set(CMAKE_PREFIX_PATH "${CMAKE_CURRENT_SOURCE_DIR}/SuperBuild/install") # move binaries to the same bin directory set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) +option(ODM_BUILD_SLAM "Build SLAM module" OFF) + # Add ODM sub-modules -add_subdirectory(modules) \ No newline at end of file +add_subdirectory(modules) diff --git a/SuperBuild/CMakeLists.txt b/SuperBuild/CMakeLists.txt index 5d77b648..f7a68d19 100644 --- a/SuperBuild/CMakeLists.txt +++ b/SuperBuild/CMakeLists.txt @@ -11,6 +11,8 @@ set(CMAKE_MODULE_PATH ${SB_ROOT_DIR}/cmake) include(ExternalProject) include(ExternalProject-Setup) +option(ODM_BUILD_SLAM "Build SLAM module" OFF) + ################################ # Setup SuperBuild directories # @@ -101,10 +103,14 @@ set(custom_libs OpenGV CMVS Catkin Ecto - PDAL - LAStools + PDAL) + +# Dependencies of the SLAM module +if(ODM_BUILD_SLAM) + list(APPEND custom_libs Pangolin ORB_SLAM2) +endif() foreach(lib ${custom_libs}) SETUP_EXTERNAL_PROJECT_CUSTOM(${lib}) diff --git a/SuperBuild/cmake/External-OpenCV.cmake b/SuperBuild/cmake/External-OpenCV.cmake index 0e39d6b1..232059c9 100644 --- a/SuperBuild/cmake/External-OpenCV.cmake +++ b/SuperBuild/cmake/External-OpenCV.cmake @@ -27,9 +27,9 @@ ExternalProject_Add(${_proj_name} -DBUILD_opencv_photo=ON -DBUILD_opencv_legacy=ON -DBUILD_opencv_python=ON - -DWITH_FFMPEG=ON + -DWITH_FFMPEG=${ODM_BUILD_SLAM} -DWITH_CUDA=OFF - -DWITH_GTK=ON + -DWITH_GTK=${ODM_BUILD_SLAM} -DWITH_VTK=OFF -DWITH_EIGEN=OFF -DWITH_OPENNI=OFF diff --git a/modules/CMakeLists.txt b/modules/CMakeLists.txt index 54d4f04d..d4f15cfe 100644 --- a/modules/CMakeLists.txt +++ b/modules/CMakeLists.txt @@ -4,4 +4,6 @@ add_subdirectory(odm_georef) add_subdirectory(odm_meshing) add_subdirectory(odm_orthophoto) add_subdirectory(odm_texturing) -add_subdirectory(odm_slam) +if (ODM_BUILD_SLAM) + add_subdirectory(odm_slam) +endif () From ed0695dddea015c4ade0f8400f4fd56624c1acd9 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Fri, 16 Dec 2016 11:34:32 +0100 Subject: [PATCH 22/28] Add link directory for libpangolin --- modules/odm_slam/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/modules/odm_slam/CMakeLists.txt b/modules/odm_slam/CMakeLists.txt index df97d53c..18eefa45 100644 --- a/modules/odm_slam/CMakeLists.txt +++ b/modules/odm_slam/CMakeLists.txt @@ -23,11 +23,14 @@ include_directories(${OpenCV_INCLUDE_DIRS}) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -std=c++11") +set(PANGOLIN_ROOT ${CMAKE_BINARY_DIR}/../SuperBuild/install) + set(ORB_SLAM_ROOT ${CMAKE_BINARY_DIR}/../SuperBuild/src/orb_slam2) include_directories(${EIGEN_ROOT}) include_directories(${ORB_SLAM_ROOT}) include_directories(${ORB_SLAM_ROOT}/include) +link_directories(${PANGOLIN_ROOT}/lib) link_directories(${ORB_SLAM_ROOT}/lib) # Add source directory From d7cd3f4a05e4efe29986e861723524aaab5a2357 Mon Sep 17 00:00:00 2001 From: Dakota Benjamin Date: Fri, 23 Dec 2016 20:50:26 +0000 Subject: [PATCH 23/28] Add note to README --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index f6f4236a..d03212d2 100644 --- a/README.md +++ b/README.md @@ -162,6 +162,10 @@ To pass in custom parameters to the run.py script, simply pass it as arguments t A web interface and API to OpenDroneMap is currently under active development in the [WebODM](https://github.com/OpenDroneMap/WebODM) repository. +## Video Support + +Currently we have an experimental feature that uses ORB_SLAM to render a textured mesh from video. It is only supported on Ubuntu 14.04 on machines with X11 support. See the [wiki](https://github.com/OpenDroneMap/OpenDroneMap/wiki/Reconstruction-from-Video)for details on installation and use. + ## Examples Coming soon... From 55671f0047f2aed1ed3797434c7dc4634b7818b3 Mon Sep 17 00:00:00 2001 From: Mark Hale Date: Thu, 29 Dec 2016 22:31:39 +0000 Subject: [PATCH 24/28] Multithreaded georef - EA --- modules/odm_georef/src/Georef.cpp | 34 ++++++++++++++++++++++++++++--- modules/odm_georef/src/Georef.hpp | 16 +++++++++++++++ 2 files changed, 47 insertions(+), 3 deletions(-) diff --git a/modules/odm_georef/src/Georef.cpp b/modules/odm_georef/src/Georef.cpp index d56ec2ae..f84d0b20 100644 --- a/modules/odm_georef/src/Georef.cpp +++ b/modules/odm_georef/src/Georef.cpp @@ -1185,9 +1185,39 @@ void Georef::chooseBestGCPTriplet(size_t &gcp0, size_t &gcp1, size_t &gcp2) void Georef::chooseBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2) { + size_t numThreads = 2; + boost::thread_group threads; + std::vector triplets; + for(size_t t = 0; t < numThreads; ++t) + { + GeorefBestTriplet triplet; + triplets.push_back(triplet); + threads.create_thread(boost::bind(&Georef::findBestCameraTriplet, this, boost::ref(triplet.t_), boost::ref(triplet.s_), boost::ref(triplet.p_), t, numThreads, boost::ref(triplet.err_))); + } + + threads.join_all(); + double minTotError = std::numeric_limits::infinity(); + for(size_t t = 0; t triplet.err_) + { + minTotError = triplet.err_; + cam0 = triplet.t_; + cam1 = triplet.s_; + cam2 = triplet.p_; + } + } + + log_ << "Mean georeference error " << minTotError / static_cast(cameras_.size()) << '\n'; +} + +void Georef::findBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2, size_t offset, size_t stride, double &minTotError) +{ + minTotError = std::numeric_limits::infinity(); - for(size_t t = 0; t < cameras_.size(); ++t) + for(size_t t = offset; t < cameras_.size(); t+=stride) { for(size_t s = t; s < cameras_.size(); ++s) { @@ -1215,8 +1245,6 @@ void Georef::chooseBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2) } } } - - log_ << "Mean georeference error " << minTotError / static_cast(cameras_.size()) << '\n'; } void Georef::printGeorefSystem() diff --git a/modules/odm_georef/src/Georef.hpp b/modules/odm_georef/src/Georef.hpp index 2cdb884c..b9a56cbd 100644 --- a/modules/odm_georef/src/Georef.hpp +++ b/modules/odm_georef/src/Georef.hpp @@ -111,6 +111,17 @@ struct GeorefCamera friend std::ostream& operator<<(std::ostream &os, const GeorefCamera &cam); }; +/*! + * \brief The GeorefBestTriplet struct is used to store the best triplet found. + */ +struct GeorefBestTriplet +{ + size_t t_; /**< First ordinate of the best triplet found. **/ + size_t s_; /**< Second ordinate of the best triplet found. **/ + size_t p_; /**< Third ordinate of the best triplet found. **/ + double err_; /**< Error of this triplet. **/ +}; + /*! * \brief The Georef class is used to transform a mesh into a georeferenced system. * The class reads camera positions from a bundle file. @@ -200,6 +211,11 @@ private: * \brief chooseBestCameraTriplet Chooses the best triplet of cameras to use when making the model georeferenced. */ void chooseBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2); + + /*! + * \brief chooseBestCameraTriplet Partitioned version of chooseBestCameraTriplet. + */ + void findBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2, size_t offset, size_t stride, double &minTotError); /*! * \brief printGeorefSystem Prints a file containing information about the georeference system, next to the ouptut file. From 6f34a759f64943e4bd7456bf75a97e3d3363d95e Mon Sep 17 00:00:00 2001 From: Mark Hale Date: Fri, 30 Dec 2016 22:31:29 +0000 Subject: [PATCH 25/28] Improvements and auto detection of numThreads. --- modules/odm_georef/src/Georef.cpp | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/modules/odm_georef/src/Georef.cpp b/modules/odm_georef/src/Georef.cpp index f84d0b20..22bf2663 100644 --- a/modules/odm_georef/src/Georef.cpp +++ b/modules/odm_georef/src/Georef.cpp @@ -1185,14 +1185,14 @@ void Georef::chooseBestGCPTriplet(size_t &gcp0, size_t &gcp1, size_t &gcp2) void Georef::chooseBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2) { - size_t numThreads = 2; + size_t numThreads = boost::thread::hardware_concurrency(); boost::thread_group threads; - std::vector triplets; + std::vector triplets; for(size_t t = 0; t < numThreads; ++t) { - GeorefBestTriplet triplet; + GeorefBestTriplet* triplet = new GeorefBestTriplet(); triplets.push_back(triplet); - threads.create_thread(boost::bind(&Georef::findBestCameraTriplet, this, boost::ref(triplet.t_), boost::ref(triplet.s_), boost::ref(triplet.p_), t, numThreads, boost::ref(triplet.err_))); + threads.create_thread(boost::bind(&Georef::findBestCameraTriplet, this, boost::ref(triplet->t_), boost::ref(triplet->s_), boost::ref(triplet->p_), t, numThreads, boost::ref(triplet->err_))); } threads.join_all(); @@ -1200,14 +1200,15 @@ void Georef::chooseBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2) double minTotError = std::numeric_limits::infinity(); for(size_t t = 0; t triplet.err_) + GeorefBestTriplet* triplet = triplets[t]; + if(minTotError > triplet->err_) { - minTotError = triplet.err_; - cam0 = triplet.t_; - cam1 = triplet.s_; - cam2 = triplet.p_; + minTotError = triplet->err_; + cam0 = triplet->t_; + cam1 = triplet->s_; + cam2 = triplet->p_; } + delete triplet; } log_ << "Mean georeference error " << minTotError / static_cast(cameras_.size()) << '\n'; @@ -1245,6 +1246,9 @@ void Georef::findBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2, siz } } } + + log_ << '[' << offset+1 << " of " << stride << "] Mean georeference error " << minTotError / static_cast(cameras_.size()); + log_ << " (" << cam0 << ", " << cam1 << ", " << cam2 << ")\n"; } void Georef::printGeorefSystem() From d40db13273d7155f25ef3f019796c0f68d56bb9c Mon Sep 17 00:00:00 2001 From: Mark Hale Date: Sat, 31 Dec 2016 02:59:05 +0000 Subject: [PATCH 26/28] Multi-threaded GCP too. --- modules/odm_georef/src/Georef.cpp | 39 ++++++++++++++++++++++++++++--- modules/odm_georef/src/Georef.hpp | 7 +++++- 2 files changed, 42 insertions(+), 4 deletions(-) diff --git a/modules/odm_georef/src/Georef.cpp b/modules/odm_georef/src/Georef.cpp index 22bf2663..c7c6e891 100644 --- a/modules/odm_georef/src/Georef.cpp +++ b/modules/odm_georef/src/Georef.cpp @@ -1141,9 +1141,40 @@ void Georef::createGeoreferencedModelFromExifData() void Georef::chooseBestGCPTriplet(size_t &gcp0, size_t &gcp1, size_t &gcp2) { - double minTotError = std::numeric_limits::infinity(); + size_t numThreads = boost::thread::hardware_concurrency(); + boost::thread_group threads; + std::vector triplets; + for(size_t t = 0; t < numThreads; ++t) + { + GeorefBestTriplet* triplet = new GeorefBestTriplet(); + triplets.push_back(triplet); + threads.create_thread(boost::bind(&Georef::findBestGCPTriplet, this, boost::ref(triplet->t_), boost::ref(triplet->s_), boost::ref(triplet->p_), t, numThreads, boost::ref(triplet->err_))); + } - for(size_t t = 0; t < gcps_.size(); ++t) + threads.join_all(); + + double minTotError = std::numeric_limits::infinity(); + for(size_t t = 0; t triplet->err_) + { + minTotError = triplet->err_; + gcp0 = triplet->t_; + gcp1 = triplet->s_; + gcp2 = triplet->p_; + } + delete triplet; + } + + log_ << "Mean georeference error " << minTotError / static_cast(gcps_.size()) << '\n'; +} + +void Georef::findBestGCPTriplet(size_t &gcp0, size_t &gcp1, size_t &gcp2, size_t offset, size_t stride, double &minTotError) +{ + minTotError = std::numeric_limits::infinity(); + + for(size_t t = offset; t < gcps_.size(); t+=stride) { if (gcps_[t].use_) { @@ -1180,7 +1211,9 @@ void Georef::chooseBestGCPTriplet(size_t &gcp0, size_t &gcp1, size_t &gcp2) } } } - log_ << "Mean georeference error " << minTotError / static_cast(cameras_.size()) << '\n'; + + log_ << '[' << offset+1 << " of " << stride << "] Mean georeference error " << minTotError / static_cast(gcps_.size()); + log_ << " (" << gcp0 << ", " << gcp1 << ", " << gcp2 << ")\n"; } void Georef::chooseBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2) diff --git a/modules/odm_georef/src/Georef.hpp b/modules/odm_georef/src/Georef.hpp index b9a56cbd..7a4ac6dd 100644 --- a/modules/odm_georef/src/Georef.hpp +++ b/modules/odm_georef/src/Georef.hpp @@ -207,13 +207,18 @@ private: */ void chooseBestGCPTriplet(size_t &gcp0, size_t &gcp1, size_t &gcp2); + /*! + * \brief findBestGCPTriplet Partitioned version of chooseBestGCPTriplet. + */ + void findBestGCPTriplet(size_t &gcp0, size_t &gcp1, size_t &gcp2, size_t offset, size_t stride, double &minTotError); + /*! * \brief chooseBestCameraTriplet Chooses the best triplet of cameras to use when making the model georeferenced. */ void chooseBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2); /*! - * \brief chooseBestCameraTriplet Partitioned version of chooseBestCameraTriplet. + * \brief findBestCameraTriplet Partitioned version of chooseBestCameraTriplet. */ void findBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2, size_t offset, size_t stride, double &minTotError); From 6c99e142bb095444ae292afdd65c229170b13dd0 Mon Sep 17 00:00:00 2001 From: Piero Toffanin Date: Mon, 2 Jan 2017 09:30:34 -0500 Subject: [PATCH 27/28] Update README.md Proposed changes for #431 --- README.md | 27 ++++++++++++--------------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index d03212d2..7f1dbff7 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,7 @@ So far, it does Point Clouds, Digital Surface Models, Textured Digital Surface M ## QUICKSTART -Requires Ubuntu 14.04 or later, see https://github.com/OpenDroneMap/odm_vagrant for running on Windows in a VM +OpenDroneMap can run natively on Ubuntu 14.04 or later, see [Build and Run Using Docker](#build-and-run-using-docker) for running on Windows / MacOS. A Vagrant VM is also available: https://github.com/OpenDroneMap/odm_vagrant. *Support for Ubuntu 12.04 is currently BROKEN with the addition of OpenSfM and Ceres-Solver. It is likely to remain broken unless a champion is found to fix it.* @@ -133,16 +133,15 @@ has equivalent procedures for Mac OS X and Windows. See [docs.docker.com](docs.d OpenDroneMap is Dockerized, meaning you can use containerization to build and run it without tampering with the configuration of libraries and packages already installed on your machine. Docker software is free to install and use in this context. If you don't have it installed, see the [Docker Ubuntu installation tutorial](https://docs.docker.com/engine/installation/linux/ubuntulinux/) and follow the -instructions through "Create a Docker group". Once Docker is installed, an OpenDroneMap Docker image can be created -like so: +instructions through "Create a Docker group". Once Docker is installed, the fastest way to use OpenDroneMap is to run a pre-built image by typing: + + docker run -it --rm -v $(pwd)/images:/code/images v $(pwd)/odm_orthophoto:/code/odm_orthophoto -v $(pwd)/odm_texturing:/code/odm_texturing opendronemap/opendronemap + +If you want to build your own Docker image from sources, type: docker build -t packages -f packages.Dockerfile . - docker build -t odm_image . - docker run -it --user root\ - -v $(pwd)/images:/code/images\ - -v $(pwd)/odm_orthophoto:/code/odm_orthophoto\ - -v $(pwd)/odm_texturing:/code/odm_texturing\ - --rm odm_image + docker build -t my_odm_image . + docker run -it --rm -v $(pwd)/images:/code/images v $(pwd)/odm_orthophoto:/code/odm_orthophoto -v $(pwd)/odm_texturing:/code/odm_texturing my_odm_image Using this method, the containerized ODM will process the images in the OpenDroneMap/images directory and output results to the OpenDroneMap/odm_orthophoto and OpenDroneMap/odm_texturing directories as described in the **Viewing Results** section. @@ -150,13 +149,11 @@ If you want to view other results outside the Docker image simply add which dire established above. For example, if you're interested in the dense cloud results generated by PMVS and in the orthophoto, simply use the following `docker run` command after building the image: - docker run -it \ - -v $(pwd)/images:/code/images\ - -v $(pwd)/pmvs:/code/pmvs\ - -v $(pwd)/odm_orthophoto:/code/odm_orthophoto\ - --rm odm_image + docker run -it --rm -v $(pwd)/images:/code/images -v $(pwd)/pmvs:/code/pmvs -v $(pwd)/odm_orthophoto:/code/odm_orthophoto my_odm_image -To pass in custom parameters to the run.py script, simply pass it as arguments to the `docker run` command. +To pass in custom parameters to the run.py script, simply pass it as arguments to the `docker run` command. For example: + + docker run -it --rm -v $(pwd)/images:/code/images v $(pwd)/odm_orthophoto:/code/odm_orthophoto -v $(pwd)/odm_texturing:/code/odm_texturing opendronemap/opendronemap --resize-to 1800 --force-ccd 6.16 ## User Interface From 7a463ab8bfe73514971e33c84852ac3cc9533643 Mon Sep 17 00:00:00 2001 From: pulquero Date: Thu, 12 Jan 2017 17:03:14 +0000 Subject: [PATCH 28/28] Make number of Opensfm processes configurable (#452) * Update README.md (#2) Proposed changes for #431 * Added support for setting number of processes for opensfm. --- opendm/config.py | 7 +++++++ scripts/odm_app.py | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/opendm/config.py b/opendm/config.py index 694e27f7..4ae7f5e5 100644 --- a/opendm/config.py +++ b/opendm/config.py @@ -118,6 +118,13 @@ def config(): 'images based on GPS exif data. Set to 0 to skip ' 'pre-matching. Default: %(default)s') + parser.add_argument('--opensfm-processes', + metavar='', + default=context.num_cores, + type=int, + help=('The maximum number of processes to use in dense ' + 'reconstruction. Default: %(default)s')) + parser.add_argument('--use-opensfm-pointcloud', action='store_true', default=False, diff --git a/scripts/odm_app.py b/scripts/odm_app.py index 66b04e05..5b3f5274 100644 --- a/scripts/odm_app.py +++ b/scripts/odm_app.py @@ -45,7 +45,7 @@ class ODMApp(ecto.BlackBox): 'opensfm': ODMOpenSfMCell(use_exif_size=False, feature_process_size=p.args.resize_to, feature_min_frames=p.args.min_num_features, - processes=context.num_cores, + processes=p.args.opensfm_processes, matching_gps_neighbors=p.args.matcher_neighbors, matching_gps_distance=p.args.matcher_distance), 'slam': ODMSlamCell(),