diff --git a/depthmap/camera.cpp b/depthmap/camera.cpp new file mode 100644 index 0000000..658f856 --- /dev/null +++ b/depthmap/camera.cpp @@ -0,0 +1,210 @@ +#include "camera.h" +#include +#include +#include +#include +#include +#include +#include "depthmap.h" +#include "../src/bni_normal_integration.h" +#include +#include +#include +#include + +using namespace std; + +bool Camera::loadXml(const QString &pathXml){ + //orientation xml + QFile file(pathXml); + if (!file.open(QIODevice::ReadOnly)) { + cerr << "Cannot open XML file: " << pathXml.toStdString() << endl; + return false; + } + + QDomDocument doc; + doc.setContent(&file); + QDomElement root = doc.documentElement(); + + QDomNodeList matrixNodes = doc.elementsByTagName("CodageMatr").at(0).childNodes(); + for (int i = 0; i < 3; ++i) { + QDomElement rowElement = matrixNodes.at(i).toElement(); + QStringList values = rowElement.text().split(" "); + + if (values.size() >= 3) { + for (int j = 0; j < 3; ++j) { + rotation(i, j) = values.at(j).toFloat(); + } + } else { + cerr << "Not enough values in row " << i << endl; + } + } + + QDomNodeList centreNodes = doc.elementsByTagName("Centre"); + if (!centreNodes.isEmpty()) { + QStringList centreValues = centreNodes.at(0).toElement().text().split(" "); + center[0] = centreValues.at(0).toFloat(); + center[1] = centreValues.at(1).toFloat(); + center[2] = centreValues.at(2).toFloat(); + } else { + cerr << "Centre not found in XML." << endl; + } + QDomElement ExportAPERO = doc.firstChildElement("ExportAPERO"); + if (ExportAPERO.isNull()) { + cerr << "ExportAPERO missing." << endl; + return false; + } + + QDomElement conique = ExportAPERO.firstChildElement("OrientationConique"); + if (conique.isNull()) { + cerr << "OrientationConique missing." << endl; + return false; + } + + + QDomElement fileInterneNode = conique.firstChildElement("FileInterne"); + if (fileInterneNode.isNull()) { + cerr << "FileInterne missing." << endl; + return false; + } + QString internePath = fileInterneNode.text(); + QFileInfo fileInfo(pathXml); + QString dirPath = fileInfo.path(); + QDir dir(dirPath); + dir.cd(".."); + + //QString interneFileName = "AutoCal_Foc-60000_Cam-NIKON_D800E.xml"; + QString fullInternePath = dir.absoluteFilePath(internePath); + if (!loadInternParameters(fullInternePath)) { + qDebug() << "Error to download the parameters to:" << fullInternePath; + return false; + } + + return true; + //autocal_foc.xml + +} + + +bool Camera::loadInternParameters(const QString &internePath){ + + QFile interneFile(internePath); + if (!interneFile.open(QIODevice::ReadOnly)) { + cerr << "Cannot open internal XML file: " << internePath.toStdString() << endl; + } + QDomDocument interneDoc; + interneDoc.setContent(&interneFile); + QDomElement interneRoot = interneDoc.documentElement(); + QDomElement calibNode = interneRoot.firstChildElement("CalibrationInternConique"); + + QDomElement focalElement = calibNode.firstChildElement("F"); + if (!focalElement.isNull()) + focal = focalElement.text().toFloat(); + else{ + cerr << "No 'F' node found in internal XML" << endl; + return false; + } + + QDomElement sizeImg = calibNode.firstChildElement("SzIm"); + if (!sizeImg.isNull()) { + QStringList dimensions = sizeImg.text().split(" "); + if (dimensions.size() == 2){ + width = dimensions[0].toUInt(); + height = dimensions[1].toUInt(); + } else { + cerr << "No 'SzIm' node found in internal XML" << endl; + return false; + } + } + QDomElement ppElement = calibNode.firstChildElement("PP"); + if (!ppElement.isNull()) { + QStringList ppValues = ppElement.text().split(" "); + if (ppValues.size() == 2){ + PPx = ppValues[0].toFloat(); + PPy = ppValues[1].toFloat(); + } else{ + cerr << "No 'PP' node found in internal XML" << endl; + return false; + } + } + QDomElement distortionNode = calibNode.firstChildElement("CalibDistortion").firstChildElement("ModRad"); + + QDomElement cDistElement = distortionNode.firstChildElement("CDist"); + if (!cDistElement.isNull()) { + QStringList cDistValues = cDistElement.text().split(" "); + if (cDistValues.size() == 2) { + Cx = cDistValues[0].toFloat(); + Cy = cDistValues[1].toFloat(); + } else { + cerr << "No 'CDist' node found in internal XML." << endl; + return false; + } + } + + QDomNodeList coeffDistNodes = distortionNode.elementsByTagName("CoeffDist"); + if (coeffDistNodes.size() >= 1) + R3 = coeffDistNodes.at(0).toElement().text().toFloat(); + if (coeffDistNodes.size() >= 2) + R5 = coeffDistNodes.at(1).toElement().text().toFloat(); + if (coeffDistNodes.size() >= 3) + R7 = coeffDistNodes.at(2).toElement().text().toFloat(); + + return true; + +} + +// Pc = Rk(Pg − Ok) +// Pg = Ground point Pc = point camera. x y z orientati come la camera, moltiplica la matrice. Poi fai la proiezione. +Eigen::Vector3f Camera::projectionToImage(Eigen::Vector3f realPosition) const{ + //centre origine + //r matrice + //matrice r inversa rotation + Eigen::Matrix3f rotationInverse = rotation.transpose(); + Eigen::Vector3f cameraCoords = rotationInverse * (realPosition - center); + + //proiezione + if (cameraCoords.z() == 0) { + cerr << "Z è zero, impossibile proiettare il punto." << endl; + return Eigen::Vector3f(0, 0, 0); + } + //Normalize by dividing by the z coordinate to get the image coordinates u and v as projected 2D coordinates + float u = cameraCoords.x() / cameraCoords.z(); + float v = cameraCoords.y() / cameraCoords.z(); + //u, v cerca manuale micmac focale + float z = cameraCoords.z(); + // + Eigen::Vector3f uvz (u, v, z); + uvz = applyIntrinsicCalibration(uvz); + //uvz = applyRadialDistortion(uvz); + return uvz; + +} + +Eigen::Vector3f Camera::applyIntrinsicCalibration(Eigen::Vector3f& uvz) const{ + + float u = uvz.x(); + float v = uvz.y(); + Eigen::Vector2f P(PPx, PPy); + Eigen::Vector2f UV(u, v); + Eigen::Vector2f result = P + focal * UV; + + return Eigen::Vector3f(result.x(), result.y(), uvz.z()); +} +// du =U−Cx dv =V −Cy ρ2 =d2u +d2v +// DRUV=Cx+(1+R3ρ2 +R5ρ4 +R7ρ6)du + +Eigen::Vector3f Camera::applyRadialDistortion(Eigen::Vector3f& uvz) { + + float du = uvz.x() - Cx; + float dv = uvz.y() - Cy; + float rho2 = du * du + dv * dv; + + float distortionFactor = 1 + R3 * rho2 + R5 * std::pow(rho2, 2) + R7 * std::pow(rho2, 3); + + float u_dist = Cx + distortionFactor * du; + float v_dist = Cy + distortionFactor * dv; + + return Eigen::Vector3f(u_dist, v_dist, uvz.z()); +} + + diff --git a/depthmap/camera.h b/depthmap/camera.h new file mode 100644 index 0000000..0ac52ae --- /dev/null +++ b/depthmap/camera.h @@ -0,0 +1,10 @@ +#ifndef CAMERA_H +#define CAMERA_H + +class Camera +{ +public: + Camera(); +}; + +#endif // CAMERA_H diff --git a/depthmap/depthmap.pro b/depthmap/depthmap.pro index 8302f9c..86028f3 100644 --- a/depthmap/depthmap.pro +++ b/depthmap/depthmap.pro @@ -11,8 +11,11 @@ CONFIG -= app_bundle LIBS += -L /opt/homebrew/lib -ltiff SOURCES += \ ../src/bni_normal_integration.cpp \ + camera.cpp \ depthmap.cpp \ - main.cpp + gaussiangrid.cpp \ + main.cpp \ + orthodepthmap.cpp mac:INCLUDEPATH += /opt/homebrew/include unix:INCLUDEPATH += ../external/eigen-3.3.9 @@ -23,4 +26,7 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin HEADERS += \ ../src/bni_normal_integration.h \ - depthmap.h + camera.h \ + depthmap.h \ + gaussiangrid.h \ + orthodepthmap.h diff --git a/depthmap/gaussiangrid.cpp b/depthmap/gaussiangrid.cpp new file mode 100644 index 0000000..1ef140c --- /dev/null +++ b/depthmap/gaussiangrid.cpp @@ -0,0 +1,3 @@ +#include "gaussiangrid.h" + +GaussianGrid::GaussianGrid() {} diff --git a/depthmap/gaussiangrid.h b/depthmap/gaussiangrid.h new file mode 100644 index 0000000..293cf92 --- /dev/null +++ b/depthmap/gaussiangrid.h @@ -0,0 +1,35 @@ +#ifndef GAUSSIANGRID_H +#define GAUSSIANGRID_H + +class GaussianGrid +{ + +public: + + GaussianGrid(); + int width, height; + std::vector values; + std::vector weights; + float sideFactor = 0.5f; // corrective factor over the 1/sqrt(n) formula. + int minSamples = 3; // minimum number of samples needed in a pixel + float sigma; + float a, b;//coefficient of linear transform from source to point cloud space. + + void init(std::vector &cloud, std::vector &source); + void fitLinear(std::vector &x, std::vector &y, float &a, float &b); //ax + b + float bilinearInterpolation(float x, float y); + void computeGaussianWeightedGrid(std::vector &differences); + void fillLaplacian(float precision); + void imageGrid(const char* filename); + //interpola la griglia per spostare la depthmap, serve per creare la funzione + float value(float x, float y); + float target(float x, float y, float source); //given a point in the source depthmap compute the z in cloud coordinate space; + + + + float depthmapToCloud(float h) { + return a*h + b; + } +}; + +#endif // GAUSSIANGRID_H diff --git a/depthmap/orthodepthmap.cpp b/depthmap/orthodepthmap.cpp new file mode 100644 index 0000000..d5e5014 --- /dev/null +++ b/depthmap/orthodepthmap.cpp @@ -0,0 +1,3 @@ +#include "orthodepthmap.h" + +OrthoDepthmap::OrthoDepthmap() {} diff --git a/depthmap/orthodepthmap.h b/depthmap/orthodepthmap.h new file mode 100644 index 0000000..17d4c51 --- /dev/null +++ b/depthmap/orthodepthmap.h @@ -0,0 +1,42 @@ +#ifndef ORTHODEPTHMAP_H +#define ORTHODEPTHMAP_H +#include +#include +#include +#include + +class OrthoDepthmap +{ +public: + OrthoDepthmap(); + + Eigen::Vector3f resolution; + Eigen::Vector3f origin; + std::vector point_cloud; + std::vector x, y, z; + std::vector z_grid; + int grid_x, grid_y; + float sigma; + + Eigen::Vector3f pixelToRealCoordinates(int pixelX, int pixelY, float pixelZ); + Eigen::Vector3f realToPixelCoord(float realX, float realY, float realZ); + bool load(const char *depth_path, const char *mask_path); + bool loadXml(const char *xmlPath); + void saveObj(const char *filename); + void projectToCameraDepthMap(const Camera& camera, const QString& outputPath); + void resizeNormals(int factorPowerOfTwo, int step = 1); + void loadPointCloud(const char *textPath); + //itera sui punti, chiama l'inversa, prima converte a intero perche sono float vede se xy stanno in w e h, se non dentro problema + //legge nella depth l h corrispondente + void verifyPointCloud(); + void integratedCamera(const CameraDepthmap& camera, const char *outputFile); + void beginIntegration(); + void endIntegration(); + + + /*1. +*/ + +}; + +#endif // ORTHODEPTHMAP_H