diff --git a/modules/odm_slam/CMakeLists.txt b/modules/odm_slam/CMakeLists.txt deleted file mode 100644 index 082534dc..00000000 --- a/modules/odm_slam/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -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(VTK 6.0 REQUIRED) -find_package(PCL 1.8 HINTS "${PCL_DIR}/share/pcl-1.8" 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) diff --git a/modules/odm_slam/src/OdmSlam.cpp b/modules/odm_slam/src/OdmSlam.cpp deleted file mode 100644 index 1fe7505d..00000000 --- a/modules/odm_slam/src/OdmSlam.cpp +++ /dev/null @@ -1,98 +0,0 @@ -#include - -#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) { - 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; -} diff --git a/modules/odm_slam/src/calibrate_video.py b/modules/odm_slam/src/calibrate_video.py deleted file mode 100644 index 59ae02aa..00000000 --- a/modules/odm_slam/src/calibrate_video.py +++ /dev/null @@ -1,152 +0,0 @@ -#!/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) diff --git a/modules/odm_slam/src/orb_slam_to_opensfm.py b/modules/odm_slam/src/orb_slam_to_opensfm.py deleted file mode 100644 index fae1feda..00000000 --- a/modules/odm_slam/src/orb_slam_to_opensfm.py +++ /dev/null @@ -1,196 +0,0 @@ -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) diff --git a/modules/odm_slam/src/undistort_radial.py b/modules/odm_slam/src/undistort_radial.py deleted file mode 100644 index db40afa7..00000000 --- a/modules/odm_slam/src/undistort_radial.py +++ /dev/null @@ -1,53 +0,0 @@ -#!/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/requirements.txt b/requirements.txt index bfcd9f3f..15bd2a9d 100644 --- a/requirements.txt +++ b/requirements.txt @@ -6,10 +6,8 @@ cryptography==3.2.1 edt==2.0.2 ExifRead==2.3.2 Fiona==1.8.17 -gpxpy==1.4.2 joblib==0.17.0 laspy==1.7.0 -loky==2.9.0 lxml==4.6.1 matplotlib==3.3.3 networkx==2.5 diff --git a/stages/odm_georeferencing.py b/stages/odm_georeferencing.py index 05bacb37..dab3f484 100644 --- a/stages/odm_georeferencing.py +++ b/stages/odm_georeferencing.py @@ -122,15 +122,14 @@ class ODMGeoreferencingStage(types.ODM_Stage): if args.fast_orthophoto: decimation_step = 10 - elif args.use_opensfm_dense: - decimation_step = 40 else: - decimation_step = 90 + decimation_step = 40 # More aggressive decimation for large datasets if not args.fast_orthophoto: decimation_step *= int(len(reconstruction.photos) / 1000) + 1 - + decimation_step = min(decimation_step, 95) + try: cropper.create_bounds_gpkg(tree.odm_georeferencing_model_laz, args.crop, decimation_step=decimation_step)