kopia lustrzana https://github.com/OpenDroneMap/ODM
Cleanup, improved cropping
Piero Toffanin
rodzic
f83c81126c
commit
140203b8cf
|
|
@ -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)
|
||||
|
|
@ -1,98 +0,0 @@
|
|||
#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;
|
||||
}
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue