kopia lustrzana https://github.com/OpenDroneMap/ODM
Export orb slam map points
Pau Gargallo
rodzic
cafc98917c
commit
6232338210
|
|
@ -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-------------
|
||||
|
|
|
|||
|
|
@ -3,6 +3,52 @@
|
|||
#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) {
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue