-
Notifications
You must be signed in to change notification settings - Fork 12
/
app_tracker.cpp
executable file
·176 lines (151 loc) · 6.47 KB
/
app_tracker.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
/**
* This file is part of LDVO.
*
* Copyright 2019 Robert Maier, Technical University of Munich.
* For more information see <https://github.com/robmaier/ldvo>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* LDVO is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* LDVO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with LDVO. If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
#include <memory>
#include <vector>
#include <Eigen/Dense>
#include <CLI/CLI.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <ldvo/dataset_tum.hpp>
#include <ldvo/timer.hpp>
#include <ldvo/timed_pose.hpp>
#include <ldvo/tracker.hpp>
#include <ldvo/version.hpp>
/**
* @brief LDVO tracker example application.
* Estimates the camera trajectory for a sequence from
* the TUM RGB-D benchmark through frame-to-frame tracking.
* @author Robert Maier
*/
int main(int argc, char *argv[])
{
// parse command line arguments using CLI11
CLI::App app("LDVO - Lightweight Dense Visual Odometry");
// dataset parameters
ldvo::DatasetTUM::Config data_cfg;
app.add_option("-i,--input", data_cfg.dataset_folder, "Input dataset folder (TUM RGB-D benchmark)")->check(CLI::ExistingDirectory);
app.add_option("-c,--cam_params", data_cfg.intrinsics_file, "Camera intrinsics parameters");
app.add_option("--max_frames", data_cfg.max_num_frames, "Maximum number of frames to load");
app.add_option("--max_depth", data_cfg.max_depth, "Maximum depth (e.g. 2.0)");
// output parameters
std::string poses_file = "";
app.add_option("-p,--poses", poses_file, "Output camera poses (TUM RGB-D benchmark format)");
// tracker parameters
ldvo::Tracker::Config tracker_cfg;
app.add_option("-l,--levels", tracker_cfg.num_levels, "Number of image pyramid levels");
app.add_option("--min_level", tracker_cfg.min_level, "Minimum pyramid level");
app.add_option("--max_level", tracker_cfg.max_level, "Maximum pyramid level");
app.add_option("--iterations", tracker_cfg.num_iterations, "Maximum number of iterations per level");
app.add_option("--update_thres", tracker_cfg.update_thres, "Update threshold for optimizer break");
// parse command line arguments
CLI11_PARSE(app, argc, argv);
// validate tracker config to make sure it is valid
tracker_cfg.validate();
// print LDVO version
std::cout << "LDVO version: " << ldvo::version() << std::endl;
// print parameters
data_cfg.print();
std::cout << "output poses: " << poses_file << std::endl;
tracker_cfg.print();
// load TUM RGB-D benchmark dataset
ldvo::DatasetTUM data(data_cfg);
if (!data.init())
{
std::cerr << "Could not initialize dataset!" << std::endl;
return 1;
}
data.camera().print();
// container for storing pose graph (poses from current frame to world)
std::vector<ldvo::TimedPose> poses;
// create image pyramids
const int w = data.camera().width();
const int h = data.camera().height();
const int num_levels = tracker_cfg.num_levels;
std::shared_ptr<ldvo::FramePyramid> prev_pyramid =
std::make_shared<ldvo::FramePyramid>(w, h, num_levels);
std::shared_ptr<ldvo::FramePyramid> cur_pyramid =
std::make_shared<ldvo::FramePyramid>(w, h, num_levels);
// create tracker
ldvo::Tracker tracker(tracker_cfg, data.camera());
// load and downsample first frame
std::shared_ptr<ldvo::Frame> prev_frame = data.loadFrame(0);
prev_pyramid->fill(*prev_frame);
// store initial camera pose for first frame
Eigen::Matrix4f pose_cur_to_world = Eigen::Matrix4f::Identity();
poses.push_back(ldvo::TimedPose(pose_cur_to_world, prev_frame->timeDepth()));
// process frames
std::cout << "processing " << data.numFrames() << " frames ..." << std::endl;
double runtime_process = 0.0;
double runtime_avg = 0.0;
int num_frames_processed = 0;
for (size_t i = 1; i < data.numFrames(); ++i)
{
std::cout << " aligning frames " << (i-1) << " and " << i << std::endl;
// load frame
std::shared_ptr<ldvo::Frame> cur_frame = data.loadFrame(i);
// measure timing for processing new frame
ldvo::Timer tmr_process;
tmr_process.start();
// downsample frame
cur_pyramid->fill(*cur_frame);
// measure timing for alignment
ldvo::Timer tmr_align;
tmr_align.start();
// align previous frame to current
Eigen::Matrix4f pose_prev_to_cur = Eigen::Matrix4f::Identity();
bool ok = tracker.align(*prev_pyramid, *cur_pyramid, pose_prev_to_cur);
// accumulate runtimes
tmr_align.stop();
runtime_avg += tmr_align.elapsed();
tmr_process.stop();
runtime_process += tmr_process.elapsed();
++num_frames_processed;
if (ok)
{
// concatenate poses and store pose
pose_cur_to_world = pose_cur_to_world * pose_prev_to_cur.inverse();
poses.push_back(ldvo::TimedPose(pose_cur_to_world, cur_frame->timeDepth()));
// switch previous and current frame
std::swap(prev_pyramid, cur_pyramid);
}
else
{
std::cerr << "could not align frames " << (i-1) << " and " << i << "!" << std::endl;
}
}
std::cout << "runtime alignment: " << (runtime_avg / num_frames_processed) * 1000.0 << " ms" << std::endl;
std::cout << "runtime process + alignment: " << (runtime_process / num_frames_processed) * 1000.0 << " ms" << std::endl;
std::cout << "frames aligned: " << poses.size() << std::endl;
// save poses
if (!poses_file.empty())
{
std::cout << "saving camera poses to " << poses_file << " ..." << std::endl;
if (!ldvo::TimedPose::save(poses_file, poses))
std::cerr << "Could not save camera poses!" << std::endl;
}
// clean up
cv::destroyAllWindows();
std::cout << "Direct Image Alignment finished." << std::endl;
return 0;
}