110 lines
4.6 KiB
C++
110 lines
4.6 KiB
C++
#include <Slicer360ToPerspective.hpp>
|
|
#include <frame_conversions.hpp>
|
|
#include <csv_parser.hpp>
|
|
#include <iostream>
|
|
|
|
#include <omp.h>
|
|
|
|
Slicer360ToPerspective::Slicer360ToPerspective(Image const& panorama_image_) : panorama_image(panorama_image_), output_image_width(2), interpolation_method(Image::InterpMethod::BILINEAR), output_folder(".")
|
|
{}
|
|
|
|
Slicer360ToPerspective & Slicer360ToPerspective::SetOutputImageSize(int width, int height) {
|
|
output_image_width = (width > 0) ? width : 1024;
|
|
output_image_height = (height > 0) ? height : output_image_width;
|
|
return *this;
|
|
}
|
|
|
|
Slicer360ToPerspective & Slicer360ToPerspective::SetInterpolationMethod(Image::InterpMethod const& interpolation_method_) {
|
|
interpolation_method = interpolation_method_;
|
|
return *this;
|
|
}
|
|
|
|
Slicer360ToPerspective & Slicer360ToPerspective::SetOutputFolder(std::string const& folder) {
|
|
output_folder = folder;
|
|
return *this;
|
|
}
|
|
|
|
Slicer360ToPerspective & Slicer360ToPerspective::SetCameras(std::vector<Camera> const& cameras_) {
|
|
cameras = cameras_;
|
|
return *this;
|
|
}
|
|
|
|
Image Slicer360ToPerspective::ComputeCoverage(int width, bool raw) const {
|
|
int height = width/2;
|
|
Image coverage(width, height, 1);
|
|
|
|
// Compute the coverage of the 360-degree image by the cameras added to the slicer.
|
|
#pragma omp parallel for
|
|
for(int i = 0; i < height; i++) {
|
|
for(int j = 0; j < width; j++) {
|
|
// Compute view vector in inertial frame from equirectangular coordinates
|
|
Eigen::Vector3d ray_dir_sph = equirectangular2sph(i, j, width, height);
|
|
Eigen::Vector3d ray_dir_cart = sph2cart(ray_dir_sph);
|
|
int count = 0;
|
|
for(Camera const& camera : cameras)
|
|
{
|
|
if(camera.IsPointVisibleInertialFrame(ray_dir_cart))
|
|
count++;
|
|
}
|
|
coverage.SetPixelValue(i, j, 0, count);
|
|
}
|
|
}
|
|
|
|
if(raw)
|
|
return coverage;
|
|
else
|
|
return coverage.Normalized().Colorized(Image::Colormap::PARULA);
|
|
}
|
|
|
|
Eigen::Vector3i Slicer360ToPerspective::ProjectToCameraPixel(Camera const& camera, int i, int j) const {
|
|
// Get the ray direction in inertial frame by projecting the pixel to the sphere
|
|
// Eigen::Vector2d Camera::PixelToNormalizedCoordinates(unsigned int i, unsigned int j, unsigned int width, unsigned int height) {
|
|
Eigen::Vector2d p_sensor = Camera::PixelToNormalizedCoordinates(i, j, output_image_width, output_image_height);
|
|
p_sensor[0] *= -1;
|
|
p_sensor[1] *= ((double)output_image_height)/((double)output_image_width);// Take aspect ratio into account.
|
|
|
|
// Compute ray direction from sensor coordinates
|
|
Eigen::Vector3d ray_dir = camera.ComputeRayDirInInertialFrame(p_sensor);
|
|
Eigen::Vector3d ray_dir_sph = cart2sph(ray_dir);
|
|
Eigen::Vector2d p_equi = sph2equirectangular_d(ray_dir_sph, panorama_image.GetWidth(), panorama_image.GetHeight());
|
|
return panorama_image.GetPixelInterp(p_equi, interpolation_method);
|
|
}
|
|
|
|
Image Slicer360ToPerspective::ProjectToCamera(Camera const& camera) const {
|
|
Image image(output_image_width, output_image_height, 3);
|
|
#pragma omp parallel for
|
|
for(int i = 0 ; i < output_image_height ; i++)
|
|
for(int j = 0 ; j < output_image_width ; j++)
|
|
image.SetPixel(i, j, ProjectToCameraPixel(camera, i, j));
|
|
return image;
|
|
}
|
|
|
|
std::vector<Image> Slicer360ToPerspective::ProjectToCameras(std::vector<Camera> const& cameras_) const {
|
|
std::vector<Image> images(cameras_.size());
|
|
#pragma omp parallel for
|
|
for(unsigned int i = 0 ; i < cameras_.size(); i++)
|
|
images[i] = ProjectToCamera(cameras_[i]);
|
|
return images;
|
|
}
|
|
|
|
std::vector<Image> Slicer360ToPerspective::ProjectToAllCameras() const {
|
|
return ProjectToCameras(cameras);
|
|
}
|
|
|
|
Slicer360ToPerspective & Slicer360ToPerspective::LoadCamerasFromFile(std::string const& filename) {
|
|
std::vector<std::vector<double>> camera_specs = parse_csv_file(filename);
|
|
if(camera_specs.size() == 0) {
|
|
std::cerr << "Error: no camera specification found in file " << filename << std::endl;
|
|
return *this;
|
|
} else if(camera_specs[0].size() < 4) {
|
|
std::cerr << "Error: camera specification in file " << filename << " must have 4 columns : yaw, pitch, roll, FOV (all in degrees)." << std::endl;
|
|
return *this;
|
|
} else if(camera_specs[0].size() > 4) {
|
|
std::cerr << "Warning: camera specification in file " << filename << " has more than 4 columns. Only the first 4 columns will be used." << std::endl;
|
|
}
|
|
// Load cameras from decoded CSV file
|
|
cameras.clear();
|
|
for(auto& camera_spec : camera_specs)
|
|
cameras.push_back(Camera::FromYawPitchRollFOV(camera_spec));
|
|
return *this;
|
|
}
|