Skip to content

Commit 39edf31

Browse files
servantftransperfectservantftransperfect
committed
Enable Sfm Pose injecting
1 parent 6cac705 commit 39edf31

File tree

2 files changed

+228
-30
lines changed

2 files changed

+228
-30
lines changed

‎meshroom/aliceVision/SfMPoseInjecting.py‎

Lines changed: 70 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,67 @@ class SfMPoseInjecting(desc.AVCommandLineNode):
1111
size = desc.DynamicNodeSize("input")
1212

1313
category = "Utils"
14-
documentation = """Use a JSON file to inject poses inside the SfMData."""
14+
documentation = """
15+
Use a JSON file to inject poses inside the SfMData.
16+
17+
[
18+
{
19+
'frame_no': 45,
20+
'rx': 1.2,
21+
'ry': -0.3,
22+
'rz': 0.1
23+
'tx': 5,
24+
'ty': 6,
25+
'tz': -2
26+
}
27+
]
28+
29+
or
30+
31+
[
32+
{
33+
'path': "image_filename",
34+
'rx': 1.2,
35+
'ry': -0.3,
36+
'rz': 0.1
37+
'tx': 5,
38+
'ty': 6,
39+
'tz': -2
40+
}
41+
]
42+
43+
or
44+
45+
[
46+
{
47+
'path': "/path/to/image",
48+
'rx': 1.2,
49+
'ry': -0.3,
50+
'rz': 0.1
51+
'tx': 5,
52+
'ty': 6,
53+
'tz': -2
54+
}
55+
]
56+
57+
58+
frame_no is the detected frameId which is set by the number found in the image filename.
59+
60+
Let's say you have a point with coordinates in the *camera frame*. The coordinates in the common world frame are given by the rotation matrix world_R_camera and the translation vector world_t_camera such that
61+
62+
worldFramePoint = world_R_camera * cameraFramePoint + world_t_camera
63+
64+
world_t_camera is defined by the triplet [tx, ty, tz] in the json file
65+
world_R_camera is defined by the triplet [rx, ry, rz] (in degrees) in the json file.
66+
67+
The matrix world_R_camera is built from the triplet, transformed using a function R depending on rotationFormat.
68+
69+
If rotationFormat is EulerZXY, then world_R_camera = R_y(ry) * R_x(rx) * R_z(rz)
70+
Where R_x(rx) is the rotation along the x axis of rx degrees, R_y(ry) is the rotation along the y axis of ry degrees, R_z(rz) is the rotation along the z axis of rz degrees. It is the ZXY euler representation.
71+
72+
Meshroom assumes that the axis x, y and z of the geometric frame in which you define the rotation and the translation is in Right hand coordinates where X points to the right, Y points downward, and Z points away from the camera. But you can change this using geometricFrame.
73+
74+
"""
1575

1676
inputs = [
1777
desc.File(
@@ -34,6 +94,15 @@ class SfMPoseInjecting(desc.AVCommandLineNode):
3494
values=["EulerZXY"],
3595
value="EulerZXY",
3696
),
97+
desc.ChoiceParam(
98+
name="geometricFrame",
99+
label="Geometric Frame",
100+
description="Defines the geometric frame for the input poses:\n"
101+
" - RHXrYbZf : Right hand coordinates, X right, Y down, Z far"
102+
" - RHXrYtZb : Right hand coordinates, X right, Y up, Z behind",
103+
values=["RHXrYbZf", "RHXrYtZb"],
104+
value="RHXrYbZf",
105+
),
37106
desc.ChoiceParam(
38107
name="verboseLevel",
39108
label="Verbose Level",

‎src/software/utils/main_sfmPoseInjecting.cpp‎

Lines changed: 158 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#include <boost/json.hpp>
1919

2020
#include <fstream>
21+
#include <filesystem>
2122

2223
// These constants define the current software version.
2324
// They must be updated when the command line is changed.
@@ -30,6 +31,7 @@ namespace po = boost::program_options;
3031
struct PoseInput
3132
{
3233
IndexT frameId;
34+
std::filesystem::path path;
3335
Eigen::Matrix4d T;
3436
};
3537

@@ -76,52 +78,141 @@ inline std::istream& operator>>(std::istream& in, ERotationFormat& s)
7678
return in;
7779
}
7880

81+
/**
82+
* I/O for Rotation format choice
83+
*/
84+
85+
enum class EGeometricFrameFormat
86+
{
87+
RHXrYbZf,
88+
RHXrYtZb
89+
};
90+
91+
inline std::string EGeometricFrameFormat_enumToString(EGeometricFrameFormat format)
92+
{
93+
switch (format)
94+
{
95+
case EGeometricFrameFormat::RHXrYbZf:
96+
{
97+
return "RHXrYbZf";
98+
}
99+
case EGeometricFrameFormat::RHXrYtZb:
100+
{
101+
return "RHXrYtZb";
102+
}
103+
}
104+
throw std::out_of_range("Invalid Geometric format type Enum: " + std::to_string(int(format)));
105+
}
106+
107+
inline EGeometricFrameFormat EGeometricFrameFormat_stringToEnum(const std::string& format)
108+
{
109+
if (format == "RHXrYbZf")
110+
{
111+
return EGeometricFrameFormat::RHXrYbZf;
112+
}
113+
114+
if (format == "RHXrYtZb")
115+
{
116+
return EGeometricFrameFormat::RHXrYtZb;
117+
}
118+
119+
throw std::out_of_range("Invalid Geometric frame format type Enum: " + format);
120+
}
121+
122+
inline std::ostream& operator<<(std::ostream& os, EGeometricFrameFormat s)
123+
{
124+
return os << EGeometricFrameFormat_enumToString(s);
125+
}
126+
127+
inline std::istream& operator>>(std::istream& in, EGeometricFrameFormat& s)
128+
{
129+
std::string token(std::istreambuf_iterator<char>(in), {});
130+
s = EGeometricFrameFormat_stringToEnum(token);
131+
return in;
132+
}
133+
134+
135+
79136
/**
80137
* @brief get a pose from a boost json object (assume the file format is ok)
81138
* @param obj the input json object
82-
* @param format the required rotation format to transform to rotation matrix
139+
* @param rotationFormat the required rotation format to transform to rotation matrix
140+
* @param geometricFrameFormat the geometric frame format used to interpret the camera pose
83141
* @param readPose the output pose information
84142
* @return false if the process failed
85143
*/
86-
bool getPoseFromJson(const boost::json::object& obj, ERotationFormat format, PoseInput& readPose)
144+
bool getPoseFromJson(const boost::json::object& obj, ERotationFormat rotationFormat, EGeometricFrameFormat & geometricFormat, PoseInput& readPose)
87145
{
88146
Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
89147

90-
if (format == ERotationFormat::EulerZXY)
148+
if (rotationFormat == ERotationFormat::EulerZXY)
91149
{
92-
// Reading information from lineup
93-
const double rx = degreeToRadian(boost::json::value_to<double>(obj.at("rx")));
94-
const double ry = degreeToRadian(boost::json::value_to<double>(obj.at("ry")));
95-
const double rz = degreeToRadian(boost::json::value_to<double>(obj.at("rz")));
150+
try
151+
{
152+
const double rx = degreeToRadian(boost::json::value_to<double>(obj.at("rx")));
153+
const double ry = degreeToRadian(boost::json::value_to<double>(obj.at("ry")));
154+
const double rz = degreeToRadian(boost::json::value_to<double>(obj.at("rz")));
96155

97-
Eigen::AngleAxisd Rx(rx, Eigen::Vector3d::UnitX());
98-
Eigen::AngleAxisd Ry(ry, Eigen::Vector3d::UnitY());
99-
Eigen::AngleAxisd Rz(rz, Eigen::Vector3d::UnitZ());
156+
Eigen::AngleAxisd Rx(rx, Eigen::Vector3d::UnitX());
157+
Eigen::AngleAxisd Ry(ry, Eigen::Vector3d::UnitY());
158+
Eigen::AngleAxisd Rz(rz, Eigen::Vector3d::UnitZ());
100159

101-
R = Ry.toRotationMatrix() * Rx.toRotationMatrix() * Rz.toRotationMatrix();
160+
R = Ry.toRotationMatrix() * Rx.toRotationMatrix() * Rz.toRotationMatrix();
161+
}
162+
catch (boost::system::system_error error)
163+
{
164+
ALICEVISION_LOG_ERROR("Incorrect rotation parameters");
165+
return false;
166+
}
102167
}
103168
else
104169
{
170+
ALICEVISION_LOG_ERROR("Unsupported rotation parameters");
105171
return false;
106172
}
107173

108-
readPose.frameId = boost::json::value_to<IndexT>(obj.at("frame_no"));
109-
110174
Eigen::Vector3d t;
111-
t.x() = boost::json::value_to<double>(obj.at("tx"));
112-
t.y() = boost::json::value_to<double>(obj.at("ty"));
113-
t.z() = boost::json::value_to<double>(obj.at("tz"));
175+
try
176+
{
177+
t.x() = boost::json::value_to<double>(obj.at("tx"));
178+
t.y() = boost::json::value_to<double>(obj.at("ty"));
179+
t.z() = boost::json::value_to<double>(obj.at("tz"));
180+
}
181+
catch (boost::system::system_error error)
182+
{
183+
//If no tx,ty,tz we assume 0 translation
184+
}
185+
186+
readPose.frameId = UndefinedIndexT;
187+
if (obj.if_contains("frame_no"))
188+
{
189+
readPose.frameId = boost::json::value_to<IndexT>(obj.at("frame_no"));
190+
}
191+
else if (obj.if_contains("path"))
192+
{
193+
readPose.path = boost::json::value_to<std::string>(obj.at("filename"));
194+
}
114195

115196
Eigen::Matrix4d world_T_camera = Eigen::Matrix4d::Identity();
116197
world_T_camera.block<3, 3>(0, 0) = R;
117198
world_T_camera.block<3, 1>(0, 3) = t;
118199

119200
//Get transform in av coordinates
120201
Eigen::Matrix4d aliceTinput = Eigen::Matrix4d::Identity();
121-
aliceTinput(1, 1) = -1;
122-
aliceTinput(1, 2) = 0;
123-
aliceTinput(2, 1) = 0;
124-
aliceTinput(2, 2) = -1;
202+
203+
switch (geometricFormat)
204+
{
205+
case EGeometricFrameFormat::RHXrYtZb:
206+
{
207+
aliceTinput(1, 1) = -1;
208+
aliceTinput(1, 2) = 0;
209+
aliceTinput(2, 1) = 0;
210+
aliceTinput(2, 2) = -1;
211+
break;
212+
}
213+
default:
214+
break;
215+
}
125216

126217

127218
world_T_camera = aliceTinput * world_T_camera * aliceTinput.inverse();
@@ -135,10 +226,11 @@ bool getPoseFromJson(const boost::json::object& obj, ERotationFormat format, Pos
135226
* The JSON file contains an array of objects. Each object describes a frameId, a rotation and a translation.
136227
* @param posesFilename the input JSON filename
137228
* @param format the required rotation format to transform to rotation matrix
229+
* @param geometricFrameFormat the geometric frame format used to interpret the camera pose
138230
* @param readPose the output poses vector
139231
* @return false if the process failed, true otherwise
140232
*/
141-
bool getPosesFromJson(const std::string& posesFilename, ERotationFormat format, std::vector<PoseInput>& readPoses)
233+
bool getPosesFromJson(const std::string& posesFilename, ERotationFormat format, EGeometricFrameFormat & geometricFormat, std::vector<PoseInput>& readPoses)
142234
{
143235
std::ifstream inputfile(posesFilename);
144236
if (!inputfile.is_open())
@@ -162,7 +254,7 @@ bool getPosesFromJson(const std::string& posesFilename, ERotationFormat format,
162254
const boost::json::object& obj = item.as_object();
163255

164256
PoseInput input;
165-
if (getPoseFromJson(obj, format, input))
257+
if (getPoseFromJson(obj, format, geometricFormat, input))
166258
{
167259
readPoses.push_back(input);
168260
}
@@ -178,6 +270,7 @@ int aliceVision_main(int argc, char** argv)
178270
std::string sfmDataOutputFilename;
179271
std::string posesFilename;
180272
ERotationFormat format;
273+
EGeometricFrameFormat gframe;
181274

182275
// clang-format off
183276
po::options_description requiredParams("Required parameters");
@@ -187,7 +280,9 @@ int aliceVision_main(int argc, char** argv)
187280
("output,o", po::value<std::string>(&sfmDataOutputFilename)->required(),
188281
"SfMData output file with the injected poses.")
189282
("rotationFormat,r", po::value<ERotationFormat>(&format)->required(),
190-
"Rotation format for the input poses: EulerZXY.");
283+
"Rotation format for the input poses: EulerZXY.")
284+
("geometricFrame, g", po::value<EGeometricFrameFormat>(&gframe)->required(),
285+
"Defines the geometric frame for the input poses");
191286

192287
po::options_description optionalParams("Optional parameters");
193288
optionalParams.add_options()
@@ -224,7 +319,7 @@ int aliceVision_main(int argc, char** argv)
224319
}
225320

226321
std::vector<PoseInput> readPoses;
227-
if (!getPosesFromJson(posesFilename, format, readPoses))
322+
if (!getPosesFromJson(posesFilename, format, gframe, readPoses))
228323
{
229324
ALICEVISION_LOG_ERROR("Cannot read the poses");
230325
return EXIT_FAILURE;
@@ -235,13 +330,47 @@ int aliceVision_main(int argc, char** argv)
235330
{
236331
for (const auto& rpose : readPoses)
237332
{
238-
if (pview->getFrameId() == rpose.frameId)
333+
//Check if the frameid is the same than described
334+
bool found = false;
335+
if (rpose.frameId != UndefinedIndexT)
239336
{
240-
geometry::Pose3 pose(rpose.T);
241-
sfmData::CameraPose cpose(pose, false);
242-
cpose.setRemovable(false);
243-
sfmData.setAbsolutePose(id, cpose);
337+
if (pview->getFrameId() == rpose.frameId)
338+
{
339+
found = true;
340+
}
244341
}
342+
343+
if (!found && !rpose.path.empty())
344+
{
345+
//Check if this view has the same filename
346+
std::filesystem::path viewPath(pview->getImage().getImagePath());
347+
if (rpose.path.has_relative_path())
348+
{
349+
if (std::filesystem::canonical(viewPath) == std::filesystem::canonical(rpose.path))
350+
{
351+
found = true;
352+
}
353+
}
354+
else
355+
{
356+
if (viewPath.filename() == rpose.path.filename())
357+
{
358+
found = true;
359+
}
360+
}
361+
}
362+
363+
if (!found)
364+
{
365+
continue;
366+
}
367+
368+
ALICEVISION_LOG_INFO("Pose assigned to image with path " << pview->getImage().getImagePath());
369+
370+
geometry::Pose3 pose(rpose.T);
371+
sfmData::CameraPose cpose(pose, false);
372+
cpose.setRemovable(false);
373+
sfmData.setAbsolutePose(id, cpose);
245374
}
246375
}
247376

0 commit comments

Comments
 (0)