forked from WebODM/OpenSplat
- Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathopensfm.cpp
More file actions
Latest commit
152 lines (121 loc) · 5.62 KB
/
Copy pathopensfm.cpp
File metadata and controls
152 lines (121 loc) · 5.62 KB
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
#include<filesystem>
#include<cstdlib>
#include<nlohmann/json.hpp>
#include"opensfm.hpp"
#include"point_io.hpp"
#include"cv_utils.hpp"
#include"tensor_math.hpp"
namespacefs= std::filesystem;
using json = nlohmann::json;
usingnamespacetorch::indexing;
namespaceosfm{
voidfrom_json(const json& j, Cam &c){
j.at("projection_type").get_to(c.projectionType);
if (j.contains("width")) j.at("width").get_to(c.width);
if (j.contains("height")) j.at("height").get_to(c.height);
if (j.contains("focal_x")) j.at("focal_x").get_to(c.fx);
if (j.contains("focal_y")) j.at("focal_y").get_to(c.fy);
if (j.contains("focal")){
j.at("focal").get_to(c.fx);
j.at("focal").get_to(c.fy);
}
if (j.contains("c_x")) j.at("c_x").get_to(c.cx);
if (j.contains("c_y")) j.at("c_y").get_to(c.cy);
if (j.contains("k1")) j.at("k1").get_to(c.k1);
if (j.contains("k2")) j.at("k2").get_to(c.k2);
if (j.contains("p1")) j.at("p1").get_to(c.p1);
if (j.contains("p2")) j.at("p2").get_to(c.p2);
if (j.contains("k3")) j.at("k3").get_to(c.k3);
}
voidfrom_json(const json& j, Shot &s){
j.at("rotation").get_to(s.rotation);
j.at("translation").get_to(s.translation);
j.at("camera").get_to(s.camera);
}
voidfrom_json(const json& j, Point &p){
j.at("coordinates").get_to(p.coordinates);
j.at("color").get_to(p.color);
}
voidfrom_json(const json& j, Reconstruction &r){
j.at("cameras").get_to(r.cameras);
j.at("shots").get_to(r.shots);
j.at("points").get_to(r.points);
}
InputData inputDataFromOpenSfM(const std::string &projectRoot){
InputData ret;
fs::path nsRoot(projectRoot);
fs::path reconstructionPath = nsRoot / "reconstruction.json";
fs::path imageListPath = nsRoot / "image_list.txt";
if (!fs::exists(reconstructionPath)) throwstd::runtime_error(reconstructionPath.string() + " does not exist");
if (!fs::exists(imageListPath)) throwstd::runtime_error(imageListPath.string() + " does not exist");
std::ifstream f(reconstructionPath.string());
json data = json::parse(f);
f.close();
std::unordered_map<std::string, std::string> images;
f.open(imageListPath.string());
std::string line;
while(std::getline(f, line)){
fs::path p(line);
if (p.is_absolute()) images[p.filename().string()] = line;
else images[p.filename().string()] = fs::absolute(nsRoot / p).string();
}
f.close();
auto reconstructions = data.templateget<std::vector<Reconstruction>>();
if (reconstructions.size() == 0) throwstd::runtime_error("No reconstructions found");
if (reconstructions.size() > 1) std::cout << "Warning: multiple OpenSfM reconstructions found, choosing the first" << std::endl;
auto reconstruction = reconstructions[0];
auto shots = reconstruction.shots;
auto cameras = reconstruction.cameras;
auto points = reconstruction.points;
torch::Tensor unorientedPoses = torch::zeros({static_cast<longint>(shots.size()), 4, 4}, torch::kFloat32);
size_t i = 0;
for (constauto &s : shots){
Shot shot = s.second;
torch::Tensor rotation = rodriguesToRotation(torch::from_blob(shot.rotation.data(), {static_cast<long>(shot.rotation.size())}, torch::kFloat32));
torch::Tensor translation = torch::from_blob(shot.translation.data(), {static_cast<long>(shot.translation.size())}, torch::kFloat32);
torch::Tensor w2c = torch::eye(4, torch::kFloat32);
w2c.index_put_({Slice(None, 3), Slice(None, 3)}, rotation);
w2c.index_put_({Slice(None, 3), Slice(3,4)}, translation.reshape({3, 1}));
unorientedPoses[i] = torch::linalg_inv(w2c);
// Convert OpenSfM's camera CRS (OpenCV) to OpenGL
unorientedPoses[i].index_put_({Slice(0, 3), Slice(1,3)}, unorientedPoses[i].index({Slice(0, 3), Slice(1,3)}) * -1.0f);
i++;
}
torch::Tensor poses = unorientedPoses;
ret.translation = torch::zeros({3});
ret.scale = 1.0f;
i = 0;
for (constauto &s : shots){
std::string filename = s.first;
Shot shot = s.second;
Cam &c = cameras[shot.camera];
if (c.projectionType != "perspective" && c.projectionType != "brown"){
throwstd::runtime_error("Camera projection type " + c.projectionType + " is not supported");
}
float normalizer = static_cast<float>((std::max)(c.width, c.height));
ret.cameras.emplace_back(Camera(c.width, c.height,
static_cast<float>(c.fx * normalizer), static_cast<float>(c.fy * normalizer),
static_cast<float>(static_cast<float>(c.width) / 2.0f + normalizer * c.cx), static_cast<float>(static_cast<float>(c.height) / 2.0f + normalizer * c.cy),
static_cast<float>(c.k1), static_cast<float>(c.k2), static_cast<float>(c.k3),
static_cast<float>(c.p1), static_cast<float>(c.p2),
poses[i++], images[filename]));
}
size_t numPoints = points.size();
torch::Tensor xyz = torch::zeros({static_cast<long>(numPoints), 3}, torch::kFloat32);
torch::Tensor rgb = torch::zeros({static_cast<long>(numPoints), 3}, torch::kUInt8);
i = 0;
for (constauto &pt: points){
Point p = pt.second;
xyz[i][0] = p.coordinates[0];
xyz[i][1] = p.coordinates[1];
xyz[i][2] = p.coordinates[2];
rgb[i][0] = static_cast<uint8_t>(p.color[0]);
rgb[i][1] = static_cast<uint8_t>(p.color[1]);
rgb[i][2] = static_cast<uint8_t>(p.color[2]);
i++;
}
ret.points.xyz = xyz;
ret.points.rgb = rgb;
return ret;
}
}