added external argument parser and euler angles conversion for camera alignment.
This commit is contained in:
parent
4c6de81a5f
commit
e20c153f1e
4 changed files with 103 additions and 6 deletions
|
|
@ -1,6 +1,6 @@
|
|||
CXX = g++
|
||||
CXXFLAGS = -std=c++17 -Wall -Wextra -pedantic -g
|
||||
INCLUDES = -I./include -ID:/Users/Jerome/Documents/Ingenierie/Programmation/eigen-3.4.0 -I./stb-master
|
||||
INCLUDES = -I./include -ID:/Users/Jerome/Documents/Ingenierie/Programmation/eigen-3.4.0 -I./stb-master -ID:/Users/Jerome/Documents/Ingenierie/Programmation/cmd_line_parser-master/include
|
||||
LDFLAGS = -fopenmp
|
||||
|
||||
SRC_DIR = src
|
||||
|
|
|
|||
|
|
@ -32,6 +32,9 @@ class Camera {
|
|||
/// @brief Orients the camera so that the forward vector points at the target. The position of the camera is (0,0,0).
|
||||
Camera & SetTarget(Eigen::Vector3d const& target);
|
||||
|
||||
/// @brief Orients the camera according to the provided Euler angles. The position of the camera is (0,0,0).
|
||||
Camera & SetEulerAngles(double yaw, double pitch, double roll, bool radians = false);
|
||||
|
||||
/// @brief Computes the ray direction in camera frame for a point (x,y) on the surface of the camera's sensor. p_norm is in normalized coordinates [-1;1].
|
||||
/// @param p_norm Normalized coordinates of the point on the sensor.
|
||||
/// @return The ray direction in camera frame, cartesian coordinates.
|
||||
|
|
|
|||
|
|
@ -67,6 +67,30 @@ Camera & Camera::SetTarget(Eigen::Vector3d const& target) {
|
|||
return *this;
|
||||
}
|
||||
|
||||
Camera & Camera::SetEulerAngles(double yaw, double pitch, double roll, bool radians) {
|
||||
if(!radians) {
|
||||
yaw *= DEG2RAD;
|
||||
pitch *= DEG2RAD;
|
||||
roll *= DEG2RAD;
|
||||
}
|
||||
|
||||
// Calculate sin and cos values
|
||||
const double s_roll = std::sin(roll);
|
||||
const double c_roll = std::cos(roll);
|
||||
const double s_pitch = std::sin(pitch);
|
||||
const double c_pitch = std::cos(pitch);
|
||||
const double s_yaw = std::sin(yaw);
|
||||
const double c_yaw = std::cos(yaw);
|
||||
|
||||
// Build rotation matrix
|
||||
R << c_pitch * c_yaw, -c_pitch * s_yaw, s_pitch,
|
||||
s_roll * s_pitch * c_yaw + c_roll * s_yaw, -s_roll * s_pitch * s_yaw + c_roll * c_yaw, -s_roll * c_pitch,
|
||||
-c_roll * s_pitch * c_yaw + s_roll * s_yaw, c_roll * s_pitch * s_yaw + s_roll * c_yaw, c_roll * c_pitch;
|
||||
|
||||
R_T = R.transpose();
|
||||
return *this;
|
||||
}
|
||||
|
||||
Eigen::Vector3d Camera::ComputeRayDirInCameraFrame(Eigen::Vector2d const& p_norm) const {
|
||||
return Eigen::Vector3d(p_norm(0)*tanFOV2, p_norm(1)*tanFOV2, -1.0).normalized();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -3,13 +3,72 @@
|
|||
#include <Image.hpp>
|
||||
#include <Slicer360ToPerspective.hpp>
|
||||
#include <frame_conversions.hpp>
|
||||
#include "../include/parser.hpp"
|
||||
|
||||
#include <test_framework.hpp>
|
||||
|
||||
int main() {
|
||||
int main(int argc, char** argv) {
|
||||
std::cout.precision(16);
|
||||
std::cout << "Running Unit Tests\n";
|
||||
|
||||
if(0){// argument parser
|
||||
std::cout << "argument parser\n";
|
||||
|
||||
// declare the parser
|
||||
cmd_line_parser::parser parser(argc, argv);
|
||||
|
||||
// configure the parser
|
||||
parser.add("perc", // name
|
||||
"A percentage value.", // description
|
||||
"-p", // shorthand
|
||||
true, // required argument
|
||||
false // not boolean option (default is false)
|
||||
);
|
||||
parser.add("input_filename", "An input file name.", "-i", true);
|
||||
|
||||
parser.add("output_filename", // name
|
||||
"An output file name.", // description
|
||||
"-o", // shorthand
|
||||
false, false);
|
||||
parser.add("num_trials", "Number of trials.", "-n", false, false);
|
||||
|
||||
parser.add("sorted", "Sort output.", "--sort", false,
|
||||
true // boolean option: a value is not expected after the shorthand
|
||||
);
|
||||
parser.add("buffered", "Buffer input.", "--buffer", false, true);
|
||||
|
||||
parser.add("ram", "Amount of ram to use.", "--ram", false, false);
|
||||
|
||||
// parse command line and return on failure
|
||||
bool success = parser.parse();
|
||||
if (!success) return 1;
|
||||
|
||||
// now get some variables
|
||||
auto perc = parser.get<float>("perc"); // deduced type is float
|
||||
auto input_filename = // deduced type is std::string
|
||||
parser.get<std::string>("input_filename");
|
||||
auto sorted_output = parser.get<bool>("sorted"); // deduced type is bool
|
||||
auto buffered_input = parser.get<bool>("buffered"); // deduced type is bool
|
||||
|
||||
size_t ram = 999; // some default value
|
||||
if (parser.parsed("ram")) {
|
||||
ram = parser.get<size_t>("ram"); // deduced type is size_t
|
||||
}
|
||||
|
||||
std::cout << "perc: " << perc << std::endl;
|
||||
std::cout << "input_filename: " << input_filename << std::endl;
|
||||
std::cout << "sorted_output: " << sorted_output << std::endl;
|
||||
std::cout << "buffered_input: " << buffered_input << std::endl;
|
||||
std::cout << "ram: " << ram << std::endl;
|
||||
|
||||
try {
|
||||
auto val = parser.get<int>("bar"); // fail: no name 'bar' was specified
|
||||
(void)val; // shut up, compiler!
|
||||
} catch (std::runtime_error const& e) {
|
||||
std::cerr << e.what() << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
if(0){// frame transforms
|
||||
std::cout << "frame transforms\n";
|
||||
// PRINT_VAR(sph2equirectangular_d(Eigen::Vector3d(1.0, 0., 0.), 100, 50).transpose());
|
||||
|
|
@ -94,7 +153,7 @@ int main() {
|
|||
}
|
||||
}
|
||||
|
||||
if(0){// Camera
|
||||
if(1){// Camera
|
||||
std::cout << "Camera\n";
|
||||
{// Constructor, accessors and setters
|
||||
std::cout << "Constructor, accessors and setters\n";
|
||||
|
|
@ -144,8 +203,10 @@ int main() {
|
|||
EXPECT_EQ(camera.GetUp(), up);
|
||||
EXPECT_EQ(camera.GetR(), R);
|
||||
EXPECT_DOUBLE_EQ(camera.GetFOV(), fov);
|
||||
|
||||
// Test point projections
|
||||
}
|
||||
{// Test point projections
|
||||
std::cout << "Test point projections\n";
|
||||
Camera camera;
|
||||
camera.SetFOV(20*DEG2RAD);
|
||||
camera.SetUp(Eigen::Vector3d(0, 0, 1));
|
||||
camera.SetTarget(Eigen::Vector3d(1.,.5,.3));
|
||||
|
|
@ -184,6 +245,15 @@ int main() {
|
|||
EXPECT_VEC_EQ(Camera::PixelToNormalizedCoordinates(height-1, width-1, width, height), Eigen::Vector2d(1.0, -1.0));
|
||||
EXPECT_VEC_EQ(Camera::PixelToNormalizedCoordinates( 0, width-1, width, height), Eigen::Vector2d(1.0, 1.0));
|
||||
}
|
||||
{// test Euler angles conversion
|
||||
std::cout << "Euler angles\n";
|
||||
PRINT_VAR(Camera().SetEulerAngles(0.0, 0.0, 0.0).GetR());
|
||||
PRINT_VAR(Camera().SetEulerAngles(45.0, 0.0, 0.0).GetR());
|
||||
PRINT_VAR(Camera().SetEulerAngles(45.0, 30.0, 0.0).GetR());
|
||||
PRINT_VAR(Camera().SetEulerAngles(0.0, 0.0, 45.0).GetR());
|
||||
|
||||
EXPECT_EQ(Camera().SetEulerAngles(45.0, 0.0, 0.0).GetR(), Camera().SetEulerAngles(45.0*DEG2RAD, 0.0, 0.0, true).GetR());// input in radians vs degrees
|
||||
}
|
||||
}
|
||||
|
||||
if(0){// Image
|
||||
|
|
@ -333,7 +403,7 @@ int main() {
|
|||
}
|
||||
}
|
||||
|
||||
if(1){// Test Slicer360ToPerspective
|
||||
if(0){// Test Slicer360ToPerspective
|
||||
if(0) { std::cout << "Coverage analysis\n";
|
||||
Slicer360ToPerspective slicer;
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue