kopia lustrzana https://github.com/OpenDroneMap/ODM
Merge branch 'orb_slam2' of https://github.com/paulinus/OpenDroneMap into paulinus-orb_slam2
Dakota Benjamin
commit
db9757cfc9
|
|
@ -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)
|
||||
add_subdirectory(modules)
|
||||
|
|
|
|||
|
|
@ -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 #
|
||||
|
|
@ -105,6 +107,13 @@ set(custom_libs OpenGV
|
|||
MvsTexturing
|
||||
)
|
||||
|
||||
# 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})
|
||||
endforeach()
|
||||
|
|
|
|||
|
|
@ -0,0 +1,78 @@
|
|||
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/paulinus/ORB_SLAM2/archive/7c11f186a53a75560cd17352d327b0bc127a82de.zip
|
||||
#--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} <SOURCE_DIR>/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} <SOURCE_DIR>/Thirdparty/g2o
|
||||
-DCMAKE_BUILD_TYPE=Release
|
||||
DEPENDEES download
|
||||
DEPENDERS build_g2o
|
||||
WORKING_DIRECTORY ${g2o_BINARY_DIR}
|
||||
ALWAYS 1
|
||||
)
|
||||
|
||||
# Uncompress Vocabulary
|
||||
ExternalProject_Add_Step(${_proj_name} uncompress_vocabulary
|
||||
COMMAND tar -xf ORBvoc.txt.tar.gz
|
||||
DEPENDEES download
|
||||
DEPENDERS configure
|
||||
WORKING_DIRECTORY <SOURCE_DIR>/Vocabulary
|
||||
ALWAYS 1
|
||||
)
|
||||
|
|
@ -27,9 +27,9 @@ ExternalProject_Add(${_proj_name}
|
|||
-DBUILD_opencv_photo=ON
|
||||
-DBUILD_opencv_legacy=ON
|
||||
-DBUILD_opencv_python=ON
|
||||
-DWITH_FFMPEG=OFF
|
||||
-DWITH_FFMPEG=${ODM_BUILD_SLAM}
|
||||
-DWITH_CUDA=OFF
|
||||
-DWITH_GTK=OFF
|
||||
-DWITH_GTK=${ODM_BUILD_SLAM}
|
||||
-DWITH_VTK=OFF
|
||||
-DWITH_EIGEN=OFF
|
||||
-DWITH_OPENNI=OFF
|
||||
|
|
@ -57,4 +57,4 @@ ExternalProject_Add(${_proj_name}
|
|||
LOG_DOWNLOAD OFF
|
||||
LOG_CONFIGURE OFF
|
||||
LOG_BUILD OFF
|
||||
)
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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/b7c66570b336e012bf3124e2a7411d417a1d35f7.zip
|
||||
URL_MD5 9b7938d1045d26b27a637b663e647aef
|
||||
#--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
|
||||
)
|
||||
|
||||
|
|
@ -4,4 +4,6 @@ add_subdirectory(odm_georef)
|
|||
add_subdirectory(odm_meshing)
|
||||
add_subdirectory(odm_orthophoto)
|
||||
add_subdirectory(odm_texturing)
|
||||
|
||||
if (ODM_BUILD_SLAM)
|
||||
add_subdirectory(odm_slam)
|
||||
endif ()
|
||||
|
|
|
|||
|
|
@ -0,0 +1,41 @@
|
|||
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(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
|
||||
aux_source_directory("./src" SRC_LIST)
|
||||
|
||||
# Add exectuteable
|
||||
add_executable(${PROJECT_NAME} ${SRC_LIST})
|
||||
target_link_libraries(odm_slam ${OpenCV_LIBS} ORB_SLAM2 pangolin)
|
||||
|
|
@ -0,0 +1,98 @@
|
|||
#include <iostream>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include <System.h>
|
||||
#include <Converter.h>
|
||||
|
||||
|
||||
void SaveKeyFrameTrajectory(ORB_SLAM2::Map *map, const string &filename, const string &tracksfile) {
|
||||
std::cout << std::endl << "Saving keyframe trajectory to " << filename << " ..." << std::endl;
|
||||
|
||||
vector<ORB_SLAM2::KeyFrame*> 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<float> q = ORB_SLAM2::Converter::toQuaternion(R);
|
||||
cv::Mat t = pKF->GetCameraCenter();
|
||||
f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at<float>(0) << " " << t.at<float>(1) << " " << t.at<float>(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<float>(0, 0)
|
||||
<< " " << coords.at<float>(1, 0)
|
||||
<< " " << coords.at<float>(2, 0)
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
f.close();
|
||||
fpoints.close();
|
||||
std::cout << std::endl << "trajectory saved!" << std::endl;
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
if(argc != 4) {
|
||||
std::cerr << std::endl <<
|
||||
"Usage: " << argv[0] << " 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);
|
||||
|
||||
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
|
||||
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;
|
||||
|
||||
SLAM.TrackMonocular(im, timestamp);
|
||||
|
||||
//usleep(int(T * 1e6));
|
||||
}
|
||||
|
||||
SLAM.Shutdown();
|
||||
SaveKeyFrameTrajectory(SLAM.GetMap(), "KeyFrameTrajectory.txt", "MapPoints.txt");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,152 @@
|
|||
#!/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.ravel()
|
||||
|
||||
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 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.")
|
||||
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
|
||||
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
rms, camera_matrix, dist_coefs = calibrator.calibrate()
|
||||
|
||||
print
|
||||
print "RMS:", rms
|
||||
print
|
||||
print orb_slam_calibration_config(camera_matrix, dist_coefs)
|
||||
|
|
@ -0,0 +1,196 @@
|
|||
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
|
||||
|
||||
|
||||
SCALE = 50
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
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'])
|
||||
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 = get_video_size(video_filename)
|
||||
size = max(width, height)
|
||||
return {
|
||||
'width': width,
|
||||
'height': height,
|
||||
'focal': np.sqrt(fx * fy) / size,
|
||||
'k1': k1,
|
||||
'k2': k2
|
||||
}
|
||||
|
||||
|
||||
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
|
||||
'''
|
||||
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], q[0], q[1], q[2]])[:3, :3].T
|
||||
t = -R.dot(c) * SCALE
|
||||
shot = {
|
||||
'camera': 'slamcam',
|
||||
'rotation': list(cv2.Rodrigues(R)[0].flat),
|
||||
'translation': list(t.flat),
|
||||
'created_at': timestamp,
|
||||
}
|
||||
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))
|
||||
height = int(cap.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT))
|
||||
cap.release()
|
||||
return width, height
|
||||
|
||||
|
||||
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
|
||||
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:
|
||||
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
|
||||
|
||||
cap.release()
|
||||
|
||||
|
||||
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(
|
||||
'points',
|
||||
help='the map points file')
|
||||
parser.add_argument(
|
||||
'config',
|
||||
help='config file with camera calibration')
|
||||
args = parser.parse_args()
|
||||
|
||||
r = {
|
||||
'cameras': {},
|
||||
'shots': {},
|
||||
'points': {},
|
||||
}
|
||||
|
||||
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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -2,7 +2,7 @@ import argparse
|
|||
import context
|
||||
|
||||
# parse arguments
|
||||
processopts = ['resize', 'opensfm', 'cmvs', 'pmvs',
|
||||
processopts = ['resize', 'opensfm', 'slam', 'cmvs', 'pmvs',
|
||||
'odm_meshing', 'mvs_texturing', 'odm_georeferencing',
|
||||
'odm_orthophoto']
|
||||
|
||||
|
|
@ -56,6 +56,14 @@ def config():
|
|||
choices=processopts,
|
||||
help=('Can be one of:' + ' | '.join(processopts)))
|
||||
|
||||
parser.add_argument('--video',
|
||||
metavar='<string>',
|
||||
help='Path to the video file to process')
|
||||
|
||||
parser.add_argument('--slam-config',
|
||||
metavar='<string>',
|
||||
help='Path to config file for orb-slam')
|
||||
|
||||
parser.add_argument('--force-focal',
|
||||
metavar='<positive float>',
|
||||
type=float,
|
||||
|
|
|
|||
|
|
@ -18,6 +18,9 @@ sys.path.append(pyopencv_path)
|
|||
opensfm_path = os.path.join(superbuild_path, "src/opensfm")
|
||||
ccd_widths_path = os.path.join(opensfm_path, 'opensfm/data/sensor_data.json')
|
||||
|
||||
# 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")
|
||||
|
|
@ -32,6 +35,7 @@ pdal_path = os.path.join(superbuild_path, 'build/pdal/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'}
|
||||
|
|
|
|||
|
|
@ -374,6 +374,8 @@ class ODM_Tree(object):
|
|||
self.odm_meshing_log = io.join_paths(self.odm_meshing, 'odm_meshing_log.txt')
|
||||
|
||||
# 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(
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@ from opendm import system
|
|||
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
|
||||
|
|
@ -47,6 +48,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,
|
||||
|
|
@ -76,7 +78,7 @@ class ODMApp(ecto.BlackBox):
|
|||
'orthophoto': ODMOrthoPhotoCell(resolution=p.args.odm_orthophoto_resolution,
|
||||
verbose=p.args.verbose)
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return cells
|
||||
|
||||
|
|
@ -92,13 +94,15 @@ 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):
|
||||
if _p.args.video:
|
||||
return self.slam_connections(_p)
|
||||
|
||||
# define initial task
|
||||
# TODO: What is this?
|
||||
# initial_task = _p.args['start_with']
|
||||
# initial_task_id = config.processopts.index(initial_task)
|
||||
|
||||
# define the connections like you would for the plasm
|
||||
# connections = []
|
||||
|
||||
# load the dataset
|
||||
connections = [self.tree[:] >> self.dataset['tree']]
|
||||
|
|
@ -151,3 +155,33 @@ class ODMApp(ecto.BlackBox):
|
|||
self.georeferencing['reconstruction'] >> self.orthophoto['reconstruction']]
|
||||
|
||||
return connections
|
||||
|
||||
def slam_connections(self, _p):
|
||||
"""Get connections used when running from video instead of images."""
|
||||
connections = []
|
||||
|
||||
# run slam cell
|
||||
connections += [self.tree[:] >> self.slam['tree'],
|
||||
self.args[:] >> self.slam['args']]
|
||||
|
||||
# run cmvs
|
||||
connections += [self.tree[:] >> self.cmvs['tree'],
|
||||
self.args[:] >> self.cmvs['args'],
|
||||
self.slam['reconstruction'] >> self.cmvs['reconstruction']]
|
||||
|
||||
# run pmvs
|
||||
connections += [self.tree[:] >> self.pmvs['tree'],
|
||||
self.args[:] >> self.pmvs['args'],
|
||||
self.cmvs['reconstruction'] >> self.pmvs['reconstruction']]
|
||||
|
||||
# create odm mesh
|
||||
connections += [self.tree[:] >> self.meshing['tree'],
|
||||
self.args[:] >> self.meshing['args'],
|
||||
self.pmvs['reconstruction'] >> self.meshing['reconstruction']]
|
||||
|
||||
# create odm texture
|
||||
connections += [self.tree[:] >> self.texturing['tree'],
|
||||
self.args[:] >> self.texturing['args'],
|
||||
self.meshing['reconstruction'] >> self.texturing['reconstruction']]
|
||||
|
||||
return connections
|
||||
|
|
|
|||
|
|
@ -0,0 +1,111 @@
|
|||
"""Cell to run odm_slam."""
|
||||
|
||||
import os
|
||||
|
||||
import ecto
|
||||
|
||||
from opendm import log
|
||||
from opendm import io
|
||||
from opendm import system
|
||||
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
|
||||
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')
|
||||
return ecto.QUIT
|
||||
|
||||
# create working directories
|
||||
system.mkdir_p(tree.opensfm)
|
||||
system.mkdir_p(tree.pmvs)
|
||||
|
||||
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'
|
||||
|
||||
# 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,
|
||||
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))
|
||||
|
||||
# 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={} {}/bin/export_bundler {}'.format(
|
||||
context.pyopencv_path, context.opensfm_path, tree.opensfm))
|
||||
else:
|
||||
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={} {}/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: {}'.format(
|
||||
tree.pmvs_visdat))
|
||||
|
||||
log.ODM_INFO('Running OMD Slam Cell - Finished')
|
||||
return ecto.OK if args.end_with != 'odm_slam' else ecto.QUIT
|
||||
|
|
@ -1,3 +1,5 @@
|
|||
import os
|
||||
|
||||
import ecto
|
||||
|
||||
from opendm import log
|
||||
|
|
@ -43,6 +45,28 @@ class ODMTexturingCell(ecto.Cell):
|
|||
(args.rerun_from is not None and
|
||||
'odm_texturing' in args.rerun_from)
|
||||
|
||||
# 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('Writing ODM Textured file in: %s'
|
||||
% tree.odm_textured_model_obj)
|
||||
|
|
@ -52,7 +76,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,
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue