From 54aa0e4dc3efac971c6c72e213bb59e9ed6f9f77 Mon Sep 17 00:00:00 2001 From: Anthony Lombardi Date: Tue, 11 Jul 2023 20:49:51 -0400 Subject: [PATCH] WIP: Add VTKCam Reader --- libautoscoper/CMakeLists.txt | 1 + libautoscoper/src/Camera.cpp | 215 +++++++++++++++++++++++++++++++---- libautoscoper/src/Camera.hpp | 7 ++ 3 files changed, 202 insertions(+), 21 deletions(-) diff --git a/libautoscoper/CMakeLists.txt b/libautoscoper/CMakeLists.txt index c8c45d8e..60201b8e 100644 --- a/libautoscoper/CMakeLists.txt +++ b/libautoscoper/CMakeLists.txt @@ -94,6 +94,7 @@ target_compile_definitions(libautoscoper PUBLIC -DUSE_LIBTIFF) target_include_directories(libautoscoper PUBLIC ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/src + ../math ) install(TARGETS libautoscoper diff --git a/libautoscoper/src/Camera.cpp b/libautoscoper/src/Camera.cpp index 4de27db2..f9e747fd 100644 --- a/libautoscoper/src/Camera.cpp +++ b/libautoscoper/src/Camera.cpp @@ -102,29 +102,37 @@ namespace xromm Camera::Camera(const std::string& mayacam) : mayacam_(mayacam) { - // Load the mayacam.csv file into an array of doubles - - std::fstream file(mayacam.c_str(), std::ios::in); - if (file.is_open() == false) { - throw std::runtime_error("File not found: " + mayacam); + // Check the file extension + std::string::size_type ext_pos = mayacam_.find_last_of('.'); + if (ext_pos == std::string::npos) { + throw std::runtime_error("Invalid MayaCam file"); } + std::string ext = mayacam_.substr(ext_pos + 1); + // if its a yaml file load it as a vtk camera + if (ext.compare("yaml") == 0) { + loadVTKCam1(mayacam_); + } + else { + std::fstream file(mayacam.c_str(), std::ios::in); + if (file.is_open() == false) { + throw std::runtime_error("File not found: " + mayacam); + } - std::string csv_line; - safeGetline(file, csv_line); - file.close(); - if (csv_line.compare("image size") == 0) - { - loadMayaCam2(mayacam); - } - else - { - loadMayaCam1(mayacam); - } - + std::string csv_line; + safeGetline(file, csv_line); + file.close(); + if (csv_line.compare("image size") == 0) + { + loadMayaCam2(mayacam); + } + else + { + loadMayaCam1(mayacam); + } + } } - -void Camera::loadMayaCam1(const std::string& mayacam) + void Camera::loadMayaCam1(const std::string& mayacam) { std::cout << "Reading MayaCam 1.0 file: " << mayacam << std::endl; @@ -210,7 +218,6 @@ void Camera::loadMayaCam1(const std::string& mayacam) calculateImagePlane(u0, v0, z); } - void Camera::loadMayaCam2(const std::string& mayacam) { std::cout << "Reading MayaCam 2.0 file: " << mayacam << std::endl; @@ -227,7 +234,6 @@ void Camera::loadMayaCam1(const std::string& mayacam) std::fstream file(mayacam.c_str(), std::ios::in); - double csv_vals[5][3]; std::string csv_line, csv_val; for (int i = 0; i < 17 && safeGetline(file, csv_line); ++i) { std::istringstream csv_line_stream(csv_line); @@ -355,6 +361,117 @@ void Camera::loadMayaCam1(const std::string& mayacam) calculateImagePlane(K[2][0], K[2][1], z); } + void Camera::loadVTKCam1(const std::string& filename) { + // Open and prase the file + double version, view_angle, image_width, image_height; + double focal_point[3], camera_position[3]; + std::fstream file(filename.c_str(), std::ios::in); + if (!file.is_open()) { + throw std::runtime_error("Could not open VTK camera file"); + } + std::string line; + // The file is a series of key value pairs separated by a colon, # denotes a comment + while (safeGetline(file, line)) { + // Ignore comments and empty lines + if (line.empty() || line[0] == '#') { + continue; + } + // Split the line into key and value + std::string key, value; + std::istringstream line_stream(line); + if (!getline(line_stream, key, ':')) { + throw std::runtime_error("Invalid VTK camera file"); + } + if (!getline(line_stream, value, ':')) { + throw std::runtime_error("Invalid VTK camera file"); + } + // Parse the key value pair + if (key == "version") { + std::istringstream value_stream(value); + if (!(value_stream >> version)) { + throw std::runtime_error("Invalid VTK camera file"); + } + } + else if (key == "focal-point") { + value.erase(std::remove(value.begin(), value.end(), '['), value.end()); + value.erase(std::remove(value.begin(), value.end(), ']'), value.end()); + value.erase(std::remove(value.begin(), value.end(), ','),value.end()); + std::istringstream value_stream(value); + if (!(value_stream >> focal_point[0])) { + throw std::runtime_error("Invalid VTK camera file"); + } + if (!(value_stream >> focal_point[1])) { + throw std::runtime_error("Invalid VTK camera file"); + } + if (!(value_stream >> focal_point[2])) { + throw std::runtime_error("Invalid VTK camera file"); + } + } + else if (key == "camera-position") { + value.erase(std::remove(value.begin(), value.end(), '['), value.end()); + value.erase(std::remove(value.begin(), value.end(), ']'), value.end()); + value.erase(std::remove(value.begin(), value.end(), ','), value.end()); + std::istringstream value_stream(value); + if (!(value_stream >> camera_position[0])) { + throw std::runtime_error("Invalid VTK camera file"); + } + if (!(value_stream >> camera_position[1])) { + throw std::runtime_error("Invalid VTK camera file"); + } + if (!(value_stream >> camera_position[2])) { + throw std::runtime_error("Invalid VTK camera file"); + } + } + else if (key == "view-angle") { + std::istringstream value_stream(value); + if (!(value_stream >> view_angle)) { + throw std::runtime_error("Invalid VTK camera file"); + } + } + else if (key == "image-width") { + std::istringstream value_stream(value); + if (!(value_stream >> image_width)) { + throw std::runtime_error("Invalid VTK camera file"); + } + } + else if (key == "image-height") { + std::istringstream value_stream(value); + if (!(value_stream >> image_height)) { + throw std::runtime_error("Invalid VTK camera file"); + } + } + } + + // Close the file + file.close(); + + if (version != 1.0) { + throw std::runtime_error("Invalid VTK camera file version, only version 1.0 is supported"); + } + + // Set the size + size_[0] = image_width; + size_[1] = image_height; + + // Get the Euler Angles + double* rotation = calculateRotationMatrix(camera_position, focal_point); // returns roation angle in XYZ order + double xyzypr[6] = { camera_position[0], camera_position[1], camera_position[2], + rotation[2], rotation[1], rotation[0] }; // Needs rotation angles in ZYX order + + // Set the coordinate frame + coord_frame_ = CoordFrame::from_xyzypr(xyzypr); + + // Calculate the focal length + double* focal_lengths = calculateFocalLength(view_angle); + + // Calculate the viewport + calculateViewport(0.0, 0.0 , focal_lengths[0], focal_lengths[1]); + + // Calculate the image plane + double z = -0.5* (focal_lengths[0] + focal_lengths[1]); + calculateImagePlane(0.0, 0.0, z); + + } void Camera::calculateViewport(const double& cx, const double& cy, const double& fx, const double& fy) { // Calculate the viewport @@ -428,4 +545,60 @@ void Camera::loadMayaCam1(const std::string& mayacam) half_height * up[2]; } + double* Camera::calculateRotationMatrix(const double* camera_pos, const double* focal_point) { + // Calculate the vector between camera_pos and the focal_point + Vec3d normal; + normal.x = focal_point[0] - camera_pos[0]; + normal.y = focal_point[1] - camera_pos[1]; + normal.z = focal_point[2] - camera_pos[2]; + + // Calculate the magnitude of the vector + double normal_magnitude = std::sqrt(dot(normal,normal)); + + // Normalize the vector + normal.x /= normal_magnitude; + normal.y /= normal_magnitude; + normal.z /= normal_magnitude; + + // Find the angle between each axis + // Calculate the angle between the normal and the x-axis + Vec3d axis_normal(1,0,0); + double dot_result = dot(normal, axis_normal); + double magnitude = len(normal) * len(axis_normal); + double angle_x = std::acos(dot_result / magnitude); + + // Calculate the angle between the normal and the y-axis + axis_normal.x = 0; // Convert to <0,1,0> + axis_normal.y = 1; + dot_result = dot(normal, axis_normal); + magnitude = len(normal) * len(axis_normal); + double angle_y = std::acos(dot_result / magnitude); + + // Calculate the angle between the normal and the z-axis + axis_normal.y = 0; // Convert to <0,0,1> + axis_normal.z = 1; + dot_result = dot(normal, axis_normal); + magnitude = len(normal) * len(axis_normal); + double angle_z = std::acos(dot_result / magnitude); + + double euler_angles[3]; + euler_angles[0] = angle_x; + euler_angles[1] = angle_y; + euler_angles[2] = angle_z; + + return euler_angles; + } + + double* Camera::calculateFocalLength(const double& view_angle) { + double focal_lengths[2]; + + // Convert from deg to rad + double angle_rad = view_angle * (M_PI / 180); + + focal_lengths[0] = size_[0] / (2 * std::tan(angle_rad / 2)); + focal_lengths[1] = size_[1] / (2 * std::tan(angle_rad / 2)); + + return focal_lengths; + } + } // namespace XROMM diff --git a/libautoscoper/src/Camera.hpp b/libautoscoper/src/Camera.hpp index 2630fd63..6ddcaa41 100644 --- a/libautoscoper/src/Camera.hpp +++ b/libautoscoper/src/Camera.hpp @@ -44,6 +44,9 @@ #include +#include +#include + #include "CoordFrame.hpp" namespace xromm @@ -89,9 +92,13 @@ class Camera void loadMayaCam2(const std::string& mayacam); + void loadVTKCam1(const std::string& filename); + // helper functions void calculateViewport(const double &cx, const double &cy, const double &fx, const double &fy); void calculateImagePlane(const double &cx, const double &cy, const double &z); + double* calculateRotationMatrix(const double* camera_pos, const double* focal_point); + double* calculateFocalLength(const double& view_angle); };