forked from PRBonn/refusion
-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.cpp
122 lines (100 loc) · 3.63 KB
/
main.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
#include <iostream>
#include <Eigen/Geometry>
#include "utils/FrameStream.h"
#include "tsdfvh/tsdf_volume.h"
#include "tracker/tracker.h"
#include <iomanip>
#include <settings.h>
#include <Logger.h>
#include <cpptoml.h>
#include <Timer.h>
using namespace std;
using namespace refusion;
int main(int argc, char** argv)
{
// Initially a new line to separate from any previous output.
cout << endl;
Timer timer {};
// For now, config file will be in a fixed dir. Here we read in all the settings.
shared_ptr<cpptoml::table> config = cpptoml::parse_file("/app/config.toml");
// Use the read in settings to create the settings structs.
settings settings = getSettings(*config);
tsdfvh::TsdfVolumeOptions tsdf_options = getTsdfVolumeOptions(*config);
TrackerOptions tracker_options = getTrackerOptions(*config);
RgbdSensor sensor = getSensorConfig(*config);
// Create the logger
Logger *logger = new Logger(settings.verbose, settings.debug, argv[1]);
logger->alwaysLog("Settings loaded. Starting reconstruction...");
logger->verboseLog("VERBOSITY ON");
logger->debugLog("DEBUG ON");
timer.addMeasurement("Settings loaded");
// Protect against no video being provided.
if (argc == 0) {
logger->error("No video provided. Ending.");
exit(EXIT_FAILURE);
}
logger->verboseLog("File provided: " + (string)argv[1] + ".");
// Create TUMVideo object
logger->verboseLog("Reading frames...");
TUMVideo video {argv[1], settings.streamFrames};
logger->verboseLog("Frames read.");
timer.addMeasurement("Frames read");
// Create tracker
logger->verboseLog("Creating tracker...");
refusion::Tracker *tracker = CreateTracker(tsdf_options, tracker_options, sensor, logger);
logger->verboseLog("Tracker created.");
timer.addMeasurement("Tracker created");
// Create output file
std::ofstream result;
if(settings.outputResults)
{
logger->verboseLog("Creating output file...");
result.open((string)argv[1] + ".txt");
logger->verboseLog("Output file created.");
}
// Start main loop
logger->verboseLog("Starting main loop...");
while (!video.finished())
{
logger->debugLog("Processing frame " + to_string(video.getCurrentFrameIndex()) + ".");
Frame frame = video.nextFrame();
logger->debugLog("Frame received from video stream.");
tracker->AddScan(frame.rgb, frame.depth);
logger->debugLog("Frame added to tracker.");
if(settings.outputResults)
{
// Get the current pose
Eigen::Matrix4d pose = tracker->GetCurrentPose();
Eigen::Quaterniond rotation(pose.block<3, 3>(0, 0));
// Write the pose to file
result << std::fixed << std::setprecision(6) << video.getCurrentTimestamp()
<< " " << pose.block<3, 1>(0, 3).transpose() << " "
<< rotation.vec().transpose() << " " << rotation.w() << std::endl;
}
if(settings.outputReprojectedVideo)
{
Mat reprojected = tracker->GenerateRgb(frame.rgb.cols, frame.rgb.rows);
logger->addFrameToOutputVideo(reprojected, "reprojected.avi");
}
}
logger->verboseLog("Main loop finished.");
timer.addMeasurement("Main loop finished", video.getFrameCount());
if (settings.outputMesh)
{
// Create mesh
logger->verboseLog("Saving mesh...");
float3 low_limits = make_float3(-3, -3, 0);
float3 high_limits = make_float3(3, 3, 4);
refusion::tsdfvh::Mesh *mesh;
cudaMallocManaged(&mesh, sizeof(refusion::tsdfvh::Mesh));
*mesh = tracker->ExtractMesh(low_limits, high_limits);
mesh->SaveToFile((string)argv[1] + ".obj");
logger->verboseLog("Mesh saved.");
timer.addMeasurement("Mesh saved");
}
if(settings.outputTimings) { logger->alwaysLog(timer.getTimingTrace()); }
logger->alwaysLog(""); // New line
logger->alwaysLog("Done.");
logger->release();
return EXIT_SUCCESS;
}