From 5b625bdcee9ba9dd416688d1a3d4c3cf32571116 Mon Sep 17 00:00:00 2001
From: Piero Toffanin <pt@masseranolabs.com>
Date: Mon, 4 Mar 2024 11:39:47 -0500
Subject: [PATCH 1/5] Load input data from X refactor

---
 CMakeLists.txt |   5 +-
 colmap.cpp     |   2 +
 colmap.hpp     |   6 ++
 input_data.cpp | 145 +++++++++++++++++++++++++++++++++++++++++++++++++
 input_data.hpp |  62 +++++++++++++++++++++
 model.cpp      |   5 --
 model.hpp      |   5 +-
 nerfstudio.cpp | 125 ------------------------------------------
 nerfstudio.hpp |  52 +-----------------
 opensplat.cpp  |  16 +++---
 10 files changed, 229 insertions(+), 194 deletions(-)
 create mode 100644 colmap.cpp
 create mode 100644 colmap.hpp
 create mode 100644 input_data.cpp
 create mode 100644 input_data.hpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index c194e5a..31beb56 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -23,6 +23,9 @@ find_package(OpenCV HINTS "${OPENCV_DIR}" REQUIRED)
 if (NOT WIN32 AND NOT APPLE)
     set(CMAKE_CUDA_COMPILER "${CUDA_TOOLKIT_ROOT_DIR}/bin/nvcc")
 endif()
+if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+    add_compile_options(-Wfatal-errors)
+endif()
 set(OpenCV_LIBS opencv_core opencv_imgproc opencv_highgui opencv_calib3d)
 
 add_library(gsplat vendor/gsplat/forward.cu vendor/gsplat/backward.cu vendor/gsplat/bindings.cu vendor/gsplat/helpers.cuh)
@@ -35,7 +38,7 @@ target_include_directories(gsplat PRIVATE
 set_target_properties(gsplat PROPERTIES LINKER_LANGUAGE CXX)
 set_target_properties(gsplat PROPERTIES CUDA_ARCHITECTURES "70;75")
 
-add_executable(opensplat opensplat.cpp point_io.cpp nerfstudio.cpp model.cpp kdtree_tensor.cpp spherical_harmonics.cpp cv_utils.cpp utils.cpp project_gaussians.cpp rasterize_gaussians.cpp ssim.cpp optim_scheduler.cpp)
+add_executable(opensplat opensplat.cpp point_io.cpp nerfstudio.cpp model.cpp kdtree_tensor.cpp spherical_harmonics.cpp cv_utils.cpp utils.cpp project_gaussians.cpp rasterize_gaussians.cpp ssim.cpp optim_scheduler.cpp colmap.cpp input_data.cpp)
 target_include_directories(opensplat PRIVATE ${PROJECT_SOURCE_DIR}/vendor/glm)
 target_link_libraries(opensplat PUBLIC ${STDPPFS_LIBRARY} cuda gsplat ${TORCH_LIBRARIES} ${OpenCV_LIBS})
 
diff --git a/colmap.cpp b/colmap.cpp
new file mode 100644
index 0000000..42dbff6
--- /dev/null
+++ b/colmap.cpp
@@ -0,0 +1,2 @@
+#include "colmap.hpp"
+
diff --git a/colmap.hpp b/colmap.hpp
new file mode 100644
index 0000000..13fe6da
--- /dev/null
+++ b/colmap.hpp
@@ -0,0 +1,6 @@
+#ifndef COLMAP_H
+#define COLMAP_H
+
+
+
+#endif
\ No newline at end of file
diff --git a/input_data.cpp b/input_data.cpp
new file mode 100644
index 0000000..289bf6f
--- /dev/null
+++ b/input_data.cpp
@@ -0,0 +1,145 @@
+#include <filesystem>
+#include "input_data.hpp"
+#include "cv_utils.hpp"
+
+namespace fs = std::filesystem;
+using namespace torch::indexing;
+
+namespace ns{
+    InputData inputDataFromNerfStudio(const std::string &projectRoot);
+}
+
+InputData inputDataFromX(const std::string &projectRoot){
+    fs::path nsRoot(projectRoot);
+
+    if (fs::exists(nsRoot / "transforms.json")){
+        return ns::inputDataFromNerfStudio(projectRoot);
+    }else{
+        throw std::runtime_error("Invalid project folder (must be either a colmap or nerfstudio project folder)");
+    }
+}
+
+torch::Tensor Camera::getIntrinsicsMatrix(){
+    return torch::tensor({{fx, 0.0f, cx},
+                          {0.0f, fy, cy},
+                          {0.0f, 0.0f, 1.0f}}, torch::kFloat32);
+}
+
+void Camera::loadImage(float downscaleFactor){
+    // Populates image and K, then updates the camera parameters
+    // Caution: this function has destructive behaviors
+    // and should be called only once
+    if (image.numel()) std::runtime_error("loadImage already called");
+    std::cout << "Loading " << filePath << std::endl;
+
+    float scaleFactor = 1.0f / downscaleFactor;
+    fx *= scaleFactor;
+    fy *= scaleFactor;
+    cx *= scaleFactor;
+    cy *= scaleFactor;
+    
+    cv::Mat cImg = imreadRGB(filePath);
+
+    if (downscaleFactor > 1.0f){
+        float f = 1.0f / downscaleFactor;
+        cv::resize(cImg, cImg, cv::Size(), f, f, cv::INTER_AREA);
+    }
+
+    K = getIntrinsicsMatrix();
+    cv::Rect roi;
+
+    if (hasDistortionParameters()){
+        // Undistort
+        std::vector<float> distCoeffs = undistortionParameters();
+        cv::Mat cK = floatNxNtensorToMat(K);
+        cv::Mat newK = cv::getOptimalNewCameraMatrix(cK, distCoeffs, cv::Size(cImg.cols, cImg.rows), 0, cv::Size(), &roi);
+
+        cv::Mat undistorted = cv::Mat::zeros(cImg.rows, cImg.cols, cImg.type());
+        cv::undistort(cImg, undistorted, cK, distCoeffs, newK);
+        
+        image = imageToTensor(undistorted);
+        K = floatNxNMatToTensor(newK);
+    }else{
+        roi = cv::Rect(0, 0, cImg.cols, cImg.rows);
+        image = imageToTensor(cImg);
+    }
+
+    // Crop to ROI
+    image = image.index({Slice(roi.y, roi.y + roi.height), Slice(roi.x, roi.x + roi.width), Slice()});
+
+    // Update parameters
+    height = image.size(0);
+    width = image.size(1);
+    fx = K[0][0].item<float>();
+    fy = K[1][1].item<float>();
+    cx = K[0][2].item<float>();
+    cy = K[1][2].item<float>();
+}
+
+torch::Tensor Camera::getImage(int downscaleFactor){
+    if (downscaleFactor <= 1) return image;
+    else{
+
+        // torch::jit::script::Module container = torch::jit::load("gt.pt");
+        // return container.attr("val").toTensor();
+
+        if (imagePyramids.find(downscaleFactor) != imagePyramids.end()){
+            return imagePyramids[downscaleFactor];
+        }
+
+        // Rescale, store and return
+        cv::Mat cImg = tensorToImage(image);
+        cv::resize(cImg, cImg, cv::Size(cImg.cols / downscaleFactor, cImg.rows / downscaleFactor), 0.0, 0.0, cv::INTER_AREA);
+        torch::Tensor t = imageToTensor(cImg);
+        imagePyramids[downscaleFactor] = t;
+        return t;
+    }
+}
+
+bool Camera::hasDistortionParameters(){
+    return k1 != 0.0f || k2 != 0.0f || k3 != 0.0f || p1 != 0.0f || p2 != 0.0f;
+}
+
+std::vector<float> Camera::undistortionParameters(){
+    std::vector<float> p = { k1, k2, p1, p2, k3, 0.0f, 0.0f, 0.0f };
+    return p;
+}
+
+void Camera::scaleOutputResolution(float scaleFactor){
+    fx = fx * scaleFactor;
+    fy = fy * scaleFactor;
+    cx = cx * scaleFactor;
+    cy = cy * scaleFactor;
+    height = static_cast<int>(static_cast<float>(height) * scaleFactor);
+    width = static_cast<int>(static_cast<float>(width) * scaleFactor);
+}
+
+std::tuple<std::vector<Camera>, Camera *> InputData::getCameras(bool validate, const std::string &valImage){
+    if (!validate) return std::make_tuple(cameras, nullptr);
+    else{
+        size_t valIdx = -1;
+        std::srand(42);
+
+        if (valImage == "random"){
+            valIdx = std::rand() % cameras.size();
+        }else{
+            for (size_t i = 0; i < cameras.size(); i++){
+                if (fs::path(cameras[i].filePath).filename().string() == valImage){
+                    valIdx = i;
+                    break;
+                }
+            }
+            if (valIdx == -1) throw std::runtime_error(valImage + " not in the list of cameras");
+        }
+
+        std::vector<Camera> cams;
+        Camera *valCam = nullptr;
+
+        for (size_t i = 0; i < cameras.size(); i++){
+            if (i != valIdx) cams.push_back(cameras[i]);
+            else valCam = &cameras[i];
+        }
+
+        return std::make_tuple(cams, valCam);
+    }
+}
diff --git a/input_data.hpp b/input_data.hpp
new file mode 100644
index 0000000..83f6cb6
--- /dev/null
+++ b/input_data.hpp
@@ -0,0 +1,62 @@
+#ifndef INPUTDATA_H
+#define INPUTDATA_H
+
+#include <iostream>
+#include <string>
+#include <fstream>
+#include <unordered_map>
+#include <opencv2/calib3d.hpp>
+#include <torch/torch.h>
+
+enum CameraType { Perspective };
+struct Camera{
+    int width = 0;
+    int height = 0;
+    float fx = 0;
+    float fy = 0;
+    float cx = 0;
+    float cy = 0;
+    float k1 = 0;
+    float k2 = 0;
+    float k3 = 0;
+    float p1 = 0;
+    float p2 = 0;
+    torch::Tensor camToWorld;
+    std::string filePath = "";
+    CameraType cameraType = CameraType::Perspective;
+
+    Camera(int width, int height, float fx, float fy, float cx, float cy, 
+        float k1, float k2, float k3, float p1, float p2,
+        const torch::Tensor &camToWorld, const std::string &filePath) : 
+        width(width), height(height), fx(fx), fy(fy), cx(cx), cy(cy), 
+        k1(k1), k2(k2), k3(k3), p1(p1), p2(p2),
+        camToWorld(camToWorld), filePath(filePath) {}
+    
+    torch::Tensor getIntrinsicsMatrix();
+    bool hasDistortionParameters();
+    std::vector<float> undistortionParameters();
+    void scaleOutputResolution(float scaleFactor);
+    torch::Tensor getImage(int downscaleFactor);
+
+    void loadImage(float downscaleFactor);
+    torch::Tensor K;
+    torch::Tensor image;
+
+    std::unordered_map<int, torch::Tensor> imagePyramids;
+};
+
+struct Points{
+    torch::Tensor xyz;
+    torch::Tensor rgb;
+};
+struct InputData{
+    std::vector<Camera> cameras;
+    float scaleFactor;
+    torch::Tensor transformMatrix;
+    Points points;
+
+    std::tuple<std::vector<Camera>, Camera *> getCameras(bool validate, const std::string &valImage = "random");
+};
+InputData inputDataFromX(const std::string &projectRoot);
+
+#endif
\ No newline at end of file
diff --git a/model.cpp b/model.cpp
index c9ff302..75acfc0 100644
--- a/model.cpp
+++ b/model.cpp
@@ -5,8 +5,6 @@
 #include "rasterize_gaussians.hpp"
 #include "vendor/gsplat/config.h"
 
-namespace ns{
-
 torch::Tensor randomQuatTensor(long long n){
     torch::Tensor u = torch::rand(n);
     torch::Tensor v = torch::rand(n);
@@ -482,6 +480,3 @@ torch::Tensor Model::mainLoss(torch::Tensor &rgb, torch::Tensor &gt, float ssimW
     torch::Tensor l1Loss = l1(rgb, gt);
     return (1.0f - ssimWeight) * l1Loss + ssimWeight * ssimLoss;
 }
-
-
-}
\ No newline at end of file
diff --git a/model.hpp b/model.hpp
index 29e373a..061574b 100644
--- a/model.hpp
+++ b/model.hpp
@@ -8,13 +8,12 @@
 #include "kdtree_tensor.hpp"
 #include "spherical_harmonics.hpp"
 #include "ssim.hpp"
+#include "input_data.hpp"
 #include "optim_scheduler.hpp"
 
 using namespace torch::indexing;
 using namespace torch::autograd;
 
-namespace ns{
-
 torch::Tensor randomQuatTensor(long long n);
 torch::Tensor quatToRotMat(const torch::Tensor &quat);
 torch::Tensor projectionMatrix(float zNear, float zFar, float fovX, float fovY, const torch::Device &device);
@@ -131,6 +130,4 @@ struct Model{
 };
 
 
-}
-
 #endif
\ No newline at end of file
diff --git a/nerfstudio.cpp b/nerfstudio.cpp
index bbbe2ad..4f98f48 100644
--- a/nerfstudio.cpp
+++ b/nerfstudio.cpp
@@ -206,129 +206,4 @@ InputData inputDataFromNerfStudio(const std::string &projectRoot){
     return ret;
 }
 
-torch::Tensor Camera::getIntrinsicsMatrix(){
-    return torch::tensor({{fx, 0.0f, cx},
-                          {0.0f, fy, cy},
-                          {0.0f, 0.0f, 1.0f}}, torch::kFloat32);
-}
-
-void Camera::loadImage(float downscaleFactor){
-    // Populates image and K, then updates the camera parameters
-    // Caution: this function has destructive behaviors
-    // and should be called only once
-    if (image.numel()) std::runtime_error("loadImage already called");
-    std::cout << "Loading " << filePath << std::endl;
-
-    float scaleFactor = 1.0f / downscaleFactor;
-    fx *= scaleFactor;
-    fy *= scaleFactor;
-    cx *= scaleFactor;
-    cy *= scaleFactor;
-    
-    cv::Mat cImg = imreadRGB(filePath);
-
-    if (downscaleFactor > 1.0f){
-        float f = 1.0f / downscaleFactor;
-        cv::resize(cImg, cImg, cv::Size(), f, f, cv::INTER_AREA);
-    }
-
-    K = getIntrinsicsMatrix();
-    cv::Rect roi;
-
-    if (hasDistortionParameters()){
-        // Undistort
-        std::vector<float> distCoeffs = undistortionParameters();
-        cv::Mat cK = floatNxNtensorToMat(K);
-        cv::Mat newK = cv::getOptimalNewCameraMatrix(cK, distCoeffs, cv::Size(cImg.cols, cImg.rows), 0, cv::Size(), &roi);
-
-        cv::Mat undistorted = cv::Mat::zeros(cImg.rows, cImg.cols, cImg.type());
-        cv::undistort(cImg, undistorted, cK, distCoeffs, newK);
-        
-        image = imageToTensor(undistorted);
-        K = floatNxNMatToTensor(newK);
-    }else{
-        roi = cv::Rect(0, 0, cImg.cols, cImg.rows);
-        image = imageToTensor(cImg);
-    }
-
-    // Crop to ROI
-    image = image.index({Slice(roi.y, roi.y + roi.height), Slice(roi.x, roi.x + roi.width), Slice()});
-
-    // Update parameters
-    height = image.size(0);
-    width = image.size(1);
-    fx = K[0][0].item<float>();
-    fy = K[1][1].item<float>();
-    cx = K[0][2].item<float>();
-    cy = K[1][2].item<float>();
-}
-
-torch::Tensor Camera::getImage(int downscaleFactor){
-    if (downscaleFactor <= 1) return image;
-    else{
-
-        // torch::jit::script::Module container = torch::jit::load("gt.pt");
-        // return container.attr("val").toTensor();
-
-        if (imagePyramids.find(downscaleFactor) != imagePyramids.end()){
-            return imagePyramids[downscaleFactor];
-        }
-
-        // Rescale, store and return
-        cv::Mat cImg = tensorToImage(image);
-        cv::resize(cImg, cImg, cv::Size(cImg.cols / downscaleFactor, cImg.rows / downscaleFactor), 0.0, 0.0, cv::INTER_AREA);
-        torch::Tensor t = imageToTensor(cImg);
-        imagePyramids[downscaleFactor] = t;
-        return t;
-    }
-}
-
-bool Camera::hasDistortionParameters(){
-    return k1 != 0.0f || k2 != 0.0f || k3 != 0.0f || p1 != 0.0f || p2 != 0.0f;
-}
-
-std::vector<float> Camera::undistortionParameters(){
-    std::vector<float> p = { k1, k2, p1, p2, k3, 0.0f, 0.0f, 0.0f };
-    return p;
-}
-
-void Camera::scaleOutputResolution(float scaleFactor){
-    fx = fx * scaleFactor;
-    fy = fy * scaleFactor;
-    cx = cx * scaleFactor;
-    cy = cy * scaleFactor;
-    height = static_cast<int>(static_cast<float>(height) * scaleFactor);
-    width = static_cast<int>(static_cast<float>(width) * scaleFactor);
-}
-
-std::tuple<std::vector<Camera>, Camera *> InputData::getCameras(bool validate, const std::string &valImage){
-    if (!validate) return std::make_tuple(cameras, nullptr);
-    else{
-        size_t valIdx = -1;
-        std::srand(42);
-
-        if (valImage == "random"){
-            valIdx = std::rand() % cameras.size();
-        }else{
-            for (size_t i = 0; i < cameras.size(); i++){
-                if (fs::path(cameras[i].filePath).filename().string() == valImage){
-                    valIdx = i;
-                    break;
-                }
-            }
-            if (valIdx == -1) throw std::runtime_error(valImage + " not in the list of cameras");
-        }
-
-        std::vector<Camera> cams;
-        Camera *valCam = nullptr;
-
-        for (size_t i = 0; i < cameras.size(); i++){
-            if (i != valIdx) cams.push_back(cameras[i]);
-            else valCam = &cameras[i];
-        }
-
-        return std::make_tuple(cams, valCam);
-    }
-}
-
 }
\ No newline at end of file
diff --git a/nerfstudio.hpp b/nerfstudio.hpp
index 32f43f3..1f0d1f4 100644
--- a/nerfstudio.hpp
+++ b/nerfstudio.hpp
@@ -4,9 +4,8 @@
 #include <iostream>
 #include <string>
 #include <fstream>
-#include <unordered_map>
 #include <torch/torch.h>
-#include <opencv2/calib3d.hpp>
+#include "input_data.hpp"
 #include "vendor/json/json_fwd.hpp"
 
 using json = nlohmann::json;
@@ -40,43 +39,6 @@ namespace ns{
     void to_json(json &j, const Transforms &t);
     void from_json(const json& j, Transforms &t);
 
-    enum CameraType { Perspective };
-    struct Camera{
-        int width = 0;
-        int height = 0;
-        float fx = 0;
-        float fy = 0;
-        float cx = 0;
-        float cy = 0;
-        float k1 = 0;
-        float k2 = 0;
-        float k3 = 0;
-        float p1 = 0;
-        float p2 = 0;
-        torch::Tensor camToWorld;
-        std::string filePath = "";
-        CameraType cameraType = CameraType::Perspective;
-
-        Camera(int width, int height, float fx, float fy, float cx, float cy, 
-            float k1, float k2, float k3, float p1, float p2,
-            const torch::Tensor &camToWorld, const std::string &filePath) : 
-            width(width), height(height), fx(fx), fy(fy), cx(cx), cy(cy), 
-            k1(k1), k2(k2), k3(k3), p1(p1), p2(p2),
-            camToWorld(camToWorld), filePath(filePath) {}
-        
-        torch::Tensor getIntrinsicsMatrix();
-        bool hasDistortionParameters();
-        std::vector<float> undistortionParameters();
-        void scaleOutputResolution(float scaleFactor);
-        torch::Tensor getImage(int downscaleFactor);
-
-        void loadImage(float downscaleFactor);
-        torch::Tensor K;
-        torch::Tensor image;
-
-        std::unordered_map<int, torch::Tensor> imagePyramids;
-    };
-
     Transforms readTransforms(const std::string &filename);
 
     torch::Tensor posesFromTransforms(const Transforms &t);
@@ -84,18 +46,6 @@ namespace ns{
 
     torch::Tensor rotationMatrix(const torch::Tensor &a, const torch::Tensor &b);
 
-    struct Points{
-        torch::Tensor xyz;
-        torch::Tensor rgb;
-    };
-    struct InputData{
-        std::vector<Camera> cameras;
-        float scaleFactor;
-        torch::Tensor transformMatrix;
-        Points points;
-
-        std::tuple<std::vector<Camera>, Camera *> getCameras(bool validate, const std::string &valImage = "random");
-    };
     InputData inputDataFromNerfStudio(const std::string &projectRoot);
 }   
 
diff --git a/opensplat.cpp b/opensplat.cpp
index 539f0a4..13ff7ea 100644
--- a/opensplat.cpp
+++ b/opensplat.cpp
@@ -1,7 +1,7 @@
 #include <filesystem>
 #include "vendor/json/json.hpp"
 #include "opensplat.hpp"
-#include "point_io.hpp"
+#include "input_data.hpp"
 #include "utils.hpp"
 #include "cv_utils.hpp"
 #include "vendor/cxxopts.hpp"
@@ -85,28 +85,28 @@ int main(int argc, char *argv[]){
     }
 
     try{
-        ns::InputData inputData = ns::inputDataFromNerfStudio(projectRoot);
-        for (ns::Camera &cam : inputData.cameras){
+        InputData inputData = inputDataFromX(projectRoot);
+        for (Camera &cam : inputData.cameras){
             cam.loadImage(downScaleFactor);
         }
 
         // Withhold a validation camera if necessary
         auto t = inputData.getCameras(validate, valImage);
-        std::vector<ns::Camera> cams = std::get<0>(t);
-        ns::Camera *valCam = std::get<1>(t);
+        std::vector<Camera> cams = std::get<0>(t);
+        Camera *valCam = std::get<1>(t);
 
-        ns::Model model(inputData.points, 
+        Model model(inputData.points, 
                         cams.size(),
                         numDownscales, resolutionSchedule, shDegree, shDegreeInterval, 
                         refineEvery, warmupLength, resetAlphaEvery, stopSplitAt, densifyGradThresh, densifySizeThresh, stopScreenSizeAt, splitScreenSize,
                         numIters,
                         device);
 
-        InfiniteRandomIterator<ns::Camera> camsIter(cams);
+        InfiniteRandomIterator<Camera> camsIter(cams);
 
         int imageSize = -1;
         for (size_t step = 1; step <= numIters; step++){
-            ns::Camera cam = camsIter.next();
+            Camera cam = camsIter.next();
 
             model.optimizersZeroGrad();
 

From 011d8e773a8c9ec7c94513882ab703bf642768cf Mon Sep 17 00:00:00 2001
From: Piero Toffanin <pt@masseranolabs.com>
Date: Mon, 4 Mar 2024 21:42:23 +0000
Subject: [PATCH 2/5] Read colmap projects

---
 CMakeLists.txt  |   5 +-
 colmap.cpp      | 145 ++++++++++++++++++++++++++++++++++++++++++++++++
 colmap.hpp      |  11 ++++
 input_data.cpp  |  11 ++--
 input_data.hpp  |   2 +
 model.cpp       |  27 +--------
 model.hpp       |   1 -
 nerfstudio.cpp  |  39 +------------
 nerfstudio.hpp  |   4 --
 point_io.cpp    |  32 +++++++++++
 point_io.hpp    |   8 +++
 tensor_math.cpp |  66 ++++++++++++++++++++++
 tensor_math.hpp |  12 ++++
 13 files changed, 285 insertions(+), 78 deletions(-)
 create mode 100644 tensor_math.cpp
 create mode 100644 tensor_math.hpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 31beb56..85956b7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -23,9 +23,6 @@ find_package(OpenCV HINTS "${OPENCV_DIR}" REQUIRED)
 if (NOT WIN32 AND NOT APPLE)
     set(CMAKE_CUDA_COMPILER "${CUDA_TOOLKIT_ROOT_DIR}/bin/nvcc")
 endif()
-if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
-    add_compile_options(-Wfatal-errors)
-endif()
 set(OpenCV_LIBS opencv_core opencv_imgproc opencv_highgui opencv_calib3d)
 
 add_library(gsplat vendor/gsplat/forward.cu vendor/gsplat/backward.cu vendor/gsplat/bindings.cu vendor/gsplat/helpers.cuh)
@@ -38,7 +35,7 @@ target_include_directories(gsplat PRIVATE
 set_target_properties(gsplat PROPERTIES LINKER_LANGUAGE CXX)
 set_target_properties(gsplat PROPERTIES CUDA_ARCHITECTURES "70;75")
 
-add_executable(opensplat opensplat.cpp point_io.cpp nerfstudio.cpp model.cpp kdtree_tensor.cpp spherical_harmonics.cpp cv_utils.cpp utils.cpp project_gaussians.cpp rasterize_gaussians.cpp ssim.cpp optim_scheduler.cpp colmap.cpp input_data.cpp)
+add_executable(opensplat opensplat.cpp point_io.cpp nerfstudio.cpp model.cpp kdtree_tensor.cpp spherical_harmonics.cpp cv_utils.cpp utils.cpp project_gaussians.cpp rasterize_gaussians.cpp ssim.cpp optim_scheduler.cpp colmap.cpp input_data.cpp tensor_math.cpp)
 target_include_directories(opensplat PRIVATE ${PROJECT_SOURCE_DIR}/vendor/glm)
 target_link_libraries(opensplat PUBLIC ${STDPPFS_LIBRARY} cuda gsplat ${TORCH_LIBRARIES} ${OpenCV_LIBS})
 
diff --git a/colmap.cpp b/colmap.cpp
index 42dbff6..17e0e35 100644
--- a/colmap.cpp
+++ b/colmap.cpp
@@ -1,2 +1,147 @@
+#include <filesystem>
 #include "colmap.hpp"
+#include "point_io.hpp"
+#include "tensor_math.hpp"
 
+namespace fs = std::filesystem;
+using namespace torch::indexing;
+
+namespace cm{
+
+InputData inputDataFromColmap(const std::string &projectRoot){
+    InputData ret;
+    fs::path cmRoot(projectRoot);
+
+    if (!fs::exists(cmRoot / "cameras.bin") && fs::exists(cmRoot / "sparse" / "0" / "cameras.bin")){
+        cmRoot = cmRoot / "sparse" / "0";
+    }
+
+    fs::path camerasPath = cmRoot / "cameras.bin";
+    fs::path imagesPath = cmRoot / "images.bin";
+    fs::path pointsPath = cmRoot / "points3D.bin";
+    
+    if (!fs::exists(camerasPath)) throw std::runtime_error(camerasPath.string() + " does not exist");
+    if (!fs::exists(imagesPath)) throw std::runtime_error(imagesPath.string() + " does not exist");
+    if (!fs::exists(pointsPath)) throw std::runtime_error(pointsPath.string() + " does not exist");
+
+    std::ifstream camf(camerasPath.string(), std::ios::binary);
+    if (!camf.is_open()) throw std::runtime_error("Cannot open " + camerasPath.string());
+    std::ifstream imgf(imagesPath.string(), std::ios::binary);
+    if (!imgf.is_open()) throw std::runtime_error("Cannot open " + imagesPath.string());
+    
+    size_t numCameras = readBinary<uint64_t>(camf);
+
+    std::unordered_map<uint32_t, Camera> camMap;
+    
+    for (size_t i = 0; i < numCameras; i++) {
+        Camera cam;
+        cam.id = readBinary<uint32_t>(camf);
+
+        CameraModel model = static_cast<CameraModel>(readBinary<int>(camf)); // model ID
+        cam.width = readBinary<uint64_t>(camf);
+        cam.height = readBinary<uint64_t>(camf);
+        
+        if (model == SimplePinhole){
+            cam.fx = readBinary<double>(camf);
+            cam.fy = cam.fx;
+            cam.cx = readBinary<double>(camf);
+            cam.cy = readBinary<double>(camf);
+        }else if (model == Pinhole){
+            cam.fx = readBinary<double>(camf);
+            cam.fy = readBinary<double>(camf);
+            cam.cx = readBinary<double>(camf);
+            cam.cy = readBinary<double>(camf);
+        }else if (model == OpenCV){
+            cam.fx = readBinary<double>(camf);
+            cam.fy = readBinary<double>(camf);
+            cam.cx = readBinary<double>(camf);
+            cam.cy = readBinary<double>(camf);
+            cam.k1 = readBinary<double>(camf);
+            cam.k2 = readBinary<double>(camf);
+            cam.p1 = readBinary<double>(camf);
+            cam.p2 = readBinary<double>(camf);
+        }else{
+            throw std::runtime_error("Unsupported camera model: " + std::to_string(model));
+        }
+
+        ret.cameras.push_back(cam);
+        camMap[cam.id] = cam;
+    }
+
+    camf.close();
+
+    torch::Tensor unorientedPoses = torch::zeros({static_cast<long int>(ret.cameras.size()), 4, 4}, torch::kFloat32);
+
+    size_t numImages = readBinary<uint64_t>(imgf);
+    for (size_t i = 0; i < numImages; i++){
+        readBinary<uint32_t>(imgf); // imageId
+        
+        torch::Tensor qVec = torch::tensor({
+            readBinary<double>(imgf),
+            readBinary<double>(imgf),
+            readBinary<double>(imgf),
+            readBinary<double>(imgf)
+        }, torch::kFloat32);
+        torch::Tensor R = quatToRotMat(qVec).transpose(0, 1);
+        torch::Tensor T = torch::tensor({
+            { readBinary<double>(imgf) },
+            { readBinary<double>(imgf) },
+            { readBinary<double>(imgf) }
+        }, torch::kFloat32);
+
+        // TODO: check cam2world matrix
+        // torch::Tensor Rinv = R.transpose(0, 1);
+        // torch::Tensor Tinv = torch::matmul(-Rinv, T);
+
+        uint32_t camId = readBinary<uint32_t>(imgf);
+
+        Camera cam = camMap[camId];
+
+        char ch = '\0';
+        std::string filePath = "";
+        while(true){
+            imgf.read(&ch, 1);
+            if (ch == '\0') break;
+            filePath += ch;
+        }
+
+        // TODO: should "images" be an option?
+        cam.filePath = (fs::path("images") / fs::path(filePath)).string();
+
+        unorientedPoses.index_put_({Slice(camId), Slice(None, 3), Slice(None, 3)}, R);
+        unorientedPoses.index_put_({Slice(camId), Slice(None, 3), Slice(3, 4)}, T);
+        
+        size_t numPoints2D = readBinary<uint64_t>(imgf);
+        for (size_t j = 0; j < numPoints2D; j++){
+            readBinary<double>(imgf); // x
+            readBinary<double>(imgf); // y
+            readBinary<uint64_t>(imgf); // point3D ID
+        }
+    }
+
+    imgf.close();
+    
+    auto r = autoOrientAndCenterPoses(unorientedPoses);
+    torch::Tensor poses = std::get<0>(r);
+    ret.transformMatrix = std::get<1>(r);
+    ret.scaleFactor = 1.0f / torch::max(torch::abs(poses.index({Slice(), Slice(None, 3), 3}))).item<float>();
+    poses.index({Slice(), Slice(None, 3), 3}) *= ret.scaleFactor;
+
+    for (size_t i = 0; i < ret.cameras.size(); i++){
+        ret.cameras[i].camToWorld = poses[i];
+    }
+
+    PointSet *pSet = readPointSet(pointsPath.string());
+    torch::Tensor points = pSet->pointsTensor().clone();
+
+    ret.points.xyz = torch::matmul(torch::cat({points, torch::ones_like(points.index({"...", Slice(None, 1)}))}, -1), 
+                    ret.transformMatrix.transpose(0, 1));
+    ret.points.xyz *= ret.scaleFactor;
+    ret.points.rgb = pSet->colorsTensor().clone();
+
+    RELEASE_POINTSET(pSet);
+
+    return ret;
+}
+
+}
\ No newline at end of file
diff --git a/colmap.hpp b/colmap.hpp
index 13fe6da..1e4e0d7 100644
--- a/colmap.hpp
+++ b/colmap.hpp
@@ -1,6 +1,17 @@
 #ifndef COLMAP_H
 #define COLMAP_H
 
+#include <fstream>
+#include "input_data.hpp"
 
+namespace cm{
+    InputData inputDataFromColmap(const std::string &projectRoot);
+
+    enum CameraModel{
+        SimplePinhole = 0, Pinhole, SimpleRadial, Radial,
+        OpenCV, OpenCVFisheye, FullOpenCV, FOV, 
+        SimpleRadialFisheye, RadialFisheye, ThinPrismFisheye
+    };
+}
 
 #endif
\ No newline at end of file
diff --git a/input_data.cpp b/input_data.cpp
index 289bf6f..b9a74bc 100644
--- a/input_data.cpp
+++ b/input_data.cpp
@@ -5,15 +5,16 @@
 namespace fs = std::filesystem;
 using namespace torch::indexing;
 
-namespace ns{
-    InputData inputDataFromNerfStudio(const std::string &projectRoot);
-}
+namespace ns{ InputData inputDataFromNerfStudio(const std::string &projectRoot); }
+namespace cm{ InputData inputDataFromColmap(const std::string &projectRoot); }
 
 InputData inputDataFromX(const std::string &projectRoot){
-    fs::path nsRoot(projectRoot);
+    fs::path root(projectRoot);
 
-    if (fs::exists(nsRoot / "transforms.json")){
+    if (fs::exists(root / "transforms.json")){
         return ns::inputDataFromNerfStudio(projectRoot);
+    }else if (fs::exists(root / "sparse") || fs::exists(root / "cameras.bin")){
+        return cm::inputDataFromColmap(projectRoot);
     }else{
         throw std::runtime_error("Invalid project folder (must be either a colmap or nerfstudio project folder)");
     }
diff --git a/input_data.hpp b/input_data.hpp
index 83f6cb6..2505c39 100644
--- a/input_data.hpp
+++ b/input_data.hpp
@@ -10,6 +10,7 @@
 
 enum CameraType { Perspective };
 struct Camera{
+    int id = -1;
     int width = 0;
     int height = 0;
     float fx = 0;
@@ -25,6 +26,7 @@ struct Camera{
     std::string filePath = "";
     CameraType cameraType = CameraType::Perspective;
 
+    Camera(){};
     Camera(int width, int height, float fx, float fy, float cx, float cy, 
         float k1, float k2, float k3, float p1, float p2,
         const torch::Tensor &camToWorld, const std::string &filePath) : 
diff --git a/model.cpp b/model.cpp
index 75acfc0..915c334 100644
--- a/model.cpp
+++ b/model.cpp
@@ -3,6 +3,7 @@
 #include "tile_bounds.hpp"
 #include "project_gaussians.hpp"
 #include "rasterize_gaussians.hpp"
+#include "tensor_math.hpp"
 #include "vendor/gsplat/config.h"
 
 torch::Tensor randomQuatTensor(long long n){
@@ -17,32 +18,6 @@ torch::Tensor randomQuatTensor(long long n){
     }, -1);
 }
 
-torch::Tensor quatToRotMat(const torch::Tensor &quat){
-    auto u = torch::unbind(torch::nn::functional::normalize(quat, torch::nn::functional::NormalizeFuncOptions().dim(-1)), -1);
-    torch::Tensor w = u[0];
-    torch::Tensor x = u[1];
-    torch::Tensor y = u[2];
-    torch::Tensor z = u[3];
-    return torch::stack({
-        torch::stack({
-            1.0 - 2.0 * (y.pow(2) + z.pow(2)),
-            2.0 * (x * y - w * z),
-            2.0 * (x * z + w * y)
-        }, -1),
-        torch::stack({
-            2.0 * (x * y + w * z),
-            1.0 - 2.0 * (x.pow(2) + z.pow(2)),
-            2.0 * (y * z - w * x)
-        }, -1),
-        torch::stack({
-            2.0 * (x * z - w * y),
-            2.0 * (y * z + w * x),
-            1.0 - 2.0 * (x.pow(2) + y.pow(2))
-        }, -1)
-    }, -2);
-    
-}
-
 torch::Tensor projectionMatrix(float zNear, float zFar, float fovX, float fovY, const torch::Device &device){
     // OpenGL perspective projection matrix
     float t = zNear * std::tan(0.5f * fovY);
diff --git a/model.hpp b/model.hpp
index 061574b..f947231 100644
--- a/model.hpp
+++ b/model.hpp
@@ -15,7 +15,6 @@ using namespace torch::indexing;
 using namespace torch::autograd;
 
 torch::Tensor randomQuatTensor(long long n);
-torch::Tensor quatToRotMat(const torch::Tensor &quat);
 torch::Tensor projectionMatrix(float zNear, float zFar, float fovX, float fovY, const torch::Device &device);
 torch::Tensor psnr(const torch::Tensor& rendered, const torch::Tensor& gt);
 torch::Tensor l1(const torch::Tensor& rendered, const torch::Tensor& gt);
diff --git a/nerfstudio.cpp b/nerfstudio.cpp
index 4f98f48..ce65a0c 100644
--- a/nerfstudio.cpp
+++ b/nerfstudio.cpp
@@ -4,6 +4,7 @@
 #include "nerfstudio.hpp"
 #include "point_io.hpp"
 #include "cv_utils.hpp"
+#include "tensor_math.hpp"
 
 namespace fs = std::filesystem;
 
@@ -123,44 +124,6 @@ torch::Tensor posesFromTransforms(const Transforms &t){
     return poses;
 }
 
-std::tuple<torch::Tensor, torch::Tensor> autoOrientAndCenterPoses(const torch::Tensor &poses){
-    // Center at mean and orient up
-    torch::Tensor origins = poses.index({"...", Slice(None, 3), 3});
-    torch::Tensor translation = torch::mean(origins, 0);
-    torch::Tensor up = torch::mean(poses.index({Slice(), Slice(None, 3), 1}), 0);
-    up = up / up.norm();
-    
-    torch::Tensor rotation = rotationMatrix(up, torch::tensor({0, 0, 1}, torch::kFloat32));
-    torch::Tensor transform = torch::cat({rotation, torch::matmul(rotation, -translation.index({"...", None}))}, -1);
-    torch::Tensor orientedPoses = torch::matmul(transform, poses);
-    return std::make_tuple(orientedPoses, transform);
-}
-
-
-torch::Tensor rotationMatrix(const torch::Tensor &a, const torch::Tensor &b){
-    // Rotation matrix that rotates vector a to vector b
-    torch::Tensor a1 = a / a.norm();
-    torch::Tensor b1 = b / b.norm();
-    torch::Tensor v = torch::cross(a1, b1);
-    torch::Tensor c = torch::dot(a1, b1);
-    const float EPS = 1e-8;
-    if (c.item<float>() < -1 + EPS){
-        torch::Tensor eps = (torch::rand(3) - 0.5f) * 0.01f;
-        return rotationMatrix(a1 + eps, b1);
-    }
-    torch::Tensor s = v.norm();
-    torch::Tensor skew = torch::zeros({3, 3}, torch::kFloat32);
-    skew[0][1] = -v[2];
-    skew[0][2] = v[1];
-    skew[1][0] = v[2];
-    skew[1][2] = -v[0];
-    skew[2][0] = -v[1];
-    skew[2][1] = v[0];
-
-    return torch::eye(3) + skew + torch::matmul(skew, skew * ((1 - c) / (s.pow(2) + EPS)));
-}
-
-
 InputData inputDataFromNerfStudio(const std::string &projectRoot){
     InputData ret;
     fs::path nsRoot(projectRoot);
diff --git a/nerfstudio.hpp b/nerfstudio.hpp
index 1f0d1f4..9ceb352 100644
--- a/nerfstudio.hpp
+++ b/nerfstudio.hpp
@@ -40,11 +40,7 @@ namespace ns{
     void from_json(const json& j, Transforms &t);
 
     Transforms readTransforms(const std::string &filename);
-
     torch::Tensor posesFromTransforms(const Transforms &t);
-    std::tuple<torch::Tensor, torch::Tensor> autoOrientAndCenterPoses(const torch::Tensor &poses);
-
-    torch::Tensor rotationMatrix(const torch::Tensor &a, const torch::Tensor &b);
 
     InputData inputDataFromNerfStudio(const std::string &projectRoot);
 }   
diff --git a/point_io.cpp b/point_io.cpp
index 89078c6..fb88301 100644
--- a/point_io.cpp
+++ b/point_io.cpp
@@ -101,6 +101,7 @@ PointSet *readPointSet(const std::string &filename) {
     PointSet *r;
     const fs::path p(filename);
     if (p.extension().string() == ".ply") r = fastPlyReadPointSet(filename);
+    else if (p.extension().string() == ".bin") r = colmapReadPointSet(filename);
     else r = pdalReadPointSet(filename);
 
     return r;
@@ -357,6 +358,37 @@ PointSet *pdalReadPointSet(const std::string &filename) {
     #endif
 }
 
+PointSet *colmapReadPointSet(const std::string &filename){
+    
+    std::ifstream reader(filename, std::ios::binary);
+    if (!reader.is_open()) throw std::runtime_error("Cannot open " + filename);
+
+    auto *r = new PointSet();
+    size_t numPoints = readBinary<uint64_t>(reader);
+    r->points.resize(numPoints);
+    r->colors.resize(numPoints);
+
+    for (size_t i = 0; i < numPoints; i++){
+        r->points[i][0] = readBinary<double>(reader);
+        r->points[i][1] = readBinary<double>(reader);
+        r->points[i][2] = readBinary<double>(reader);
+        r->colors[i][0] = readBinary<uint8_t>(reader);
+        r->colors[i][1] = readBinary<uint8_t>(reader);
+        r->colors[i][2] = readBinary<uint8_t>(reader);
+
+        readBinary<double>(reader); // error
+        size_t trackLen = readBinary<uint64_t>(reader);
+        for (size_t j = 0; j < trackLen; j++){
+            readBinary<uint32_t>(reader); // imageId
+            readBinary<uint32_t>(reader); // point2D Idx
+        }
+    }
+
+    reader.close();
+
+    return r;
+}
+
 void checkHeader(std::ifstream &reader, const std::string &prop) {
     std::string line;
     std::getline(reader, line);
diff --git a/point_io.hpp b/point_io.hpp
index d63eb60..a2d79f7 100644
--- a/point_io.hpp
+++ b/point_io.hpp
@@ -103,8 +103,16 @@ size_t getVertexCount(const std::string &line);
 inline void checkHeader(std::ifstream &reader, const std::string &prop);
 inline bool hasHeader(const std::string &line, const std::string &prop);
 
+template <typename T>
+inline T readBinary(std::ifstream &s){
+    T data;
+    s.read(reinterpret_cast<char*>(&data), sizeof(T));
+    return data;
+}
+
 PointSet *fastPlyReadPointSet(const std::string &filename);
 PointSet *pdalReadPointSet(const std::string &filename);
+PointSet *colmapReadPointSet(const std::string &filename);
 PointSet *readPointSet(const std::string &filename);
 
 void fastPlySavePointSet(PointSet &pSet, const std::string &filename);
diff --git a/tensor_math.cpp b/tensor_math.cpp
new file mode 100644
index 0000000..41cec6c
--- /dev/null
+++ b/tensor_math.cpp
@@ -0,0 +1,66 @@
+#include "tensor_math.hpp"
+
+using namespace torch::indexing;
+
+torch::Tensor quatToRotMat(const torch::Tensor &quat){
+    auto u = torch::unbind(torch::nn::functional::normalize(quat, torch::nn::functional::NormalizeFuncOptions().dim(-1)), -1);
+    torch::Tensor w = u[0];
+    torch::Tensor x = u[1];
+    torch::Tensor y = u[2];
+    torch::Tensor z = u[3];
+    return torch::stack({
+        torch::stack({
+            1.0 - 2.0 * (y.pow(2) + z.pow(2)),
+            2.0 * (x * y - w * z),
+            2.0 * (x * z + w * y)
+        }, -1),
+        torch::stack({
+            2.0 * (x * y + w * z),
+            1.0 - 2.0 * (x.pow(2) + z.pow(2)),
+            2.0 * (y * z - w * x)
+        }, -1),
+        torch::stack({
+            2.0 * (x * z - w * y),
+            2.0 * (y * z + w * x),
+            1.0 - 2.0 * (x.pow(2) + y.pow(2))
+        }, -1)
+    }, -2);
+    
+}
+
+std::tuple<torch::Tensor, torch::Tensor> autoOrientAndCenterPoses(const torch::Tensor &poses){
+    // Center at mean and orient up
+    torch::Tensor origins = poses.index({"...", Slice(None, 3), 3});
+    torch::Tensor translation = torch::mean(origins, 0);
+    torch::Tensor up = torch::mean(poses.index({Slice(), Slice(None, 3), 1}), 0);
+    up = up / up.norm();
+    
+    torch::Tensor rotation = rotationMatrix(up, torch::tensor({0, 0, 1}, torch::kFloat32));
+    torch::Tensor transform = torch::cat({rotation, torch::matmul(rotation, -translation.index({"...", None}))}, -1);
+    torch::Tensor orientedPoses = torch::matmul(transform, poses);
+    return std::make_tuple(orientedPoses, transform);
+}
+
+
+torch::Tensor rotationMatrix(const torch::Tensor &a, const torch::Tensor &b){
+    // Rotation matrix that rotates vector a to vector b
+    torch::Tensor a1 = a / a.norm();
+    torch::Tensor b1 = b / b.norm();
+    torch::Tensor v = torch::cross(a1, b1);
+    torch::Tensor c = torch::dot(a1, b1);
+    const float EPS = 1e-8;
+    if (c.item<float>() < -1 + EPS){
+        torch::Tensor eps = (torch::rand(3) - 0.5f) * 0.01f;
+        return rotationMatrix(a1 + eps, b1);
+    }
+    torch::Tensor s = v.norm();
+    torch::Tensor skew = torch::zeros({3, 3}, torch::kFloat32);
+    skew[0][1] = -v[2];
+    skew[0][2] = v[1];
+    skew[1][0] = v[2];
+    skew[1][2] = -v[0];
+    skew[2][0] = -v[1];
+    skew[2][1] = v[0];
+
+    return torch::eye(3) + skew + torch::matmul(skew, skew * ((1 - c) / (s.pow(2) + EPS)));
+}
\ No newline at end of file
diff --git a/tensor_math.hpp b/tensor_math.hpp
new file mode 100644
index 0000000..12b0849
--- /dev/null
+++ b/tensor_math.hpp
@@ -0,0 +1,12 @@
+#ifndef TENSOR_MATH_H
+#define TENSOR_MATH_H
+
+#include <torch/torch.h>
+#include <tuple>
+
+torch::Tensor quatToRotMat(const torch::Tensor &quat);
+std::tuple<torch::Tensor, torch::Tensor> autoOrientAndCenterPoses(const torch::Tensor &poses);
+torch::Tensor rotationMatrix(const torch::Tensor &a, const torch::Tensor &b);
+
+
+#endif
\ No newline at end of file

From 2fa8cab2296117e0ea729317a87671632585eb44 Mon Sep 17 00:00:00 2001
From: Piero Toffanin <pt@masseranolabs.com>
Date: Tue, 5 Mar 2024 02:51:27 +0000
Subject: [PATCH 3/5] Misc fixes

---
 colmap.cpp     | 60 +++++++++++++++++++++++++++-----------------------
 input_data.cpp | 15 ++++++++-----
 opensplat.cpp  |  8 +++++++
 point_io.cpp   |  2 ++
 4 files changed, 52 insertions(+), 33 deletions(-)

diff --git a/colmap.cpp b/colmap.cpp
index 17e0e35..5ed8444 100644
--- a/colmap.cpp
+++ b/colmap.cpp
@@ -30,49 +30,51 @@ InputData inputDataFromColmap(const std::string &projectRoot){
     if (!imgf.is_open()) throw std::runtime_error("Cannot open " + imagesPath.string());
     
     size_t numCameras = readBinary<uint64_t>(camf);
+    std::vector<Camera> cameras(numCameras);
 
-    std::unordered_map<uint32_t, Camera> camMap;
+    std::unordered_map<uint32_t, Camera *> camMap;
     
     for (size_t i = 0; i < numCameras; i++) {
-        Camera cam;
-        cam.id = readBinary<uint32_t>(camf);
+        Camera *cam = &cameras[i];
+
+        cam->id = readBinary<uint32_t>(camf);
 
         CameraModel model = static_cast<CameraModel>(readBinary<int>(camf)); // model ID
-        cam.width = readBinary<uint64_t>(camf);
-        cam.height = readBinary<uint64_t>(camf);
+        cam->width = readBinary<uint64_t>(camf);
+        cam->height = readBinary<uint64_t>(camf);
         
         if (model == SimplePinhole){
-            cam.fx = readBinary<double>(camf);
-            cam.fy = cam.fx;
-            cam.cx = readBinary<double>(camf);
-            cam.cy = readBinary<double>(camf);
+            cam->fx = readBinary<double>(camf);
+            cam->fy = cam->fx;
+            cam->cx = readBinary<double>(camf);
+            cam->cy = readBinary<double>(camf);
         }else if (model == Pinhole){
-            cam.fx = readBinary<double>(camf);
-            cam.fy = readBinary<double>(camf);
-            cam.cx = readBinary<double>(camf);
-            cam.cy = readBinary<double>(camf);
+            cam->fx = readBinary<double>(camf);
+            cam->fy = readBinary<double>(camf);
+            cam->cx = readBinary<double>(camf);
+            cam->cy = readBinary<double>(camf);
         }else if (model == OpenCV){
-            cam.fx = readBinary<double>(camf);
-            cam.fy = readBinary<double>(camf);
-            cam.cx = readBinary<double>(camf);
-            cam.cy = readBinary<double>(camf);
-            cam.k1 = readBinary<double>(camf);
-            cam.k2 = readBinary<double>(camf);
-            cam.p1 = readBinary<double>(camf);
-            cam.p2 = readBinary<double>(camf);
+            cam->fx = readBinary<double>(camf);
+            cam->fy = readBinary<double>(camf);
+            cam->cx = readBinary<double>(camf);
+            cam->cy = readBinary<double>(camf);
+            cam->k1 = readBinary<double>(camf);
+            cam->k2 = readBinary<double>(camf);
+            cam->p1 = readBinary<double>(camf);
+            cam->p2 = readBinary<double>(camf);
         }else{
             throw std::runtime_error("Unsupported camera model: " + std::to_string(model));
         }
 
-        ret.cameras.push_back(cam);
-        camMap[cam.id] = cam;
+        camMap[cam->id] = cam;
     }
 
     camf.close();
 
-    torch::Tensor unorientedPoses = torch::zeros({static_cast<long int>(ret.cameras.size()), 4, 4}, torch::kFloat32);
 
     size_t numImages = readBinary<uint64_t>(imgf);
+    torch::Tensor unorientedPoses = torch::zeros({static_cast<long int>(numImages), 4, 4}, torch::kFloat32);
+
     for (size_t i = 0; i < numImages; i++){
         readBinary<uint32_t>(imgf); // imageId
         
@@ -95,7 +97,7 @@ InputData inputDataFromColmap(const std::string &projectRoot){
 
         uint32_t camId = readBinary<uint32_t>(imgf);
 
-        Camera cam = camMap[camId];
+        Camera cam = *camMap[camId];
 
         char ch = '\0';
         std::string filePath = "";
@@ -106,10 +108,10 @@ InputData inputDataFromColmap(const std::string &projectRoot){
         }
 
         // TODO: should "images" be an option?
-        cam.filePath = (fs::path("images") / fs::path(filePath)).string();
+        cam.filePath = (fs::path(projectRoot) / "images" / filePath).string();
 
-        unorientedPoses.index_put_({Slice(camId), Slice(None, 3), Slice(None, 3)}, R);
-        unorientedPoses.index_put_({Slice(camId), Slice(None, 3), Slice(3, 4)}, T);
+        unorientedPoses.index_put_({Slice(i), Slice(None, 3), Slice(None, 3)}, R);
+        unorientedPoses.index_put_({Slice(i), Slice(None, 3), Slice(3, 4)}, T);
         
         size_t numPoints2D = readBinary<uint64_t>(imgf);
         for (size_t j = 0; j < numPoints2D; j++){
@@ -117,6 +119,8 @@ InputData inputDataFromColmap(const std::string &projectRoot){
             readBinary<double>(imgf); // y
             readBinary<uint64_t>(imgf); // point3D ID
         }
+
+        ret.cameras.push_back(cam);
     }
 
     imgf.close();
diff --git a/input_data.cpp b/input_data.cpp
index b9a74bc..bf8e045 100644
--- a/input_data.cpp
+++ b/input_data.cpp
@@ -34,12 +34,17 @@ void Camera::loadImage(float downscaleFactor){
     std::cout << "Loading " << filePath << std::endl;
 
     float scaleFactor = 1.0f / downscaleFactor;
-    fx *= scaleFactor;
-    fy *= scaleFactor;
-    cx *= scaleFactor;
-    cy *= scaleFactor;
-    
     cv::Mat cImg = imreadRGB(filePath);
+    
+    float rescaleF = 1.0f;
+    // If camera intrinsics don't match the image dimensions 
+    if (cImg.rows != height || cImg.cols != width){
+        rescaleF = static_cast<float>(cImg.rows) / static_cast<float>(height);
+    }
+    fx *= scaleFactor * rescaleF;
+    fy *= scaleFactor * rescaleF;
+    cx *= scaleFactor * rescaleF;
+    cy *= scaleFactor * rescaleF;
 
     if (downscaleFactor > 1.0f){
         float f = 1.0f / downscaleFactor;
diff --git a/opensplat.cpp b/opensplat.cpp
index 13ff7ea..de4bbc9 100644
--- a/opensplat.cpp
+++ b/opensplat.cpp
@@ -88,8 +88,16 @@ int main(int argc, char *argv[]){
         InputData inputData = inputDataFromX(projectRoot);
         for (Camera &cam : inputData.cameras){
             cam.loadImage(downScaleFactor);
+            std::cout << cam.fx << " " << cam.fy << " " << 
+                    cam.width << " " << cam.height << std::endl <<
+                    cam.cx << " " << cam.cy << std::endl <<
+                    cam.camToWorld << std::endl;
+                    
+            exit(1);
         }
 
+        
+
         // Withhold a validation camera if necessary
         auto t = inputData.getCameras(validate, valImage);
         std::vector<Camera> cams = std::get<0>(t);
diff --git a/point_io.cpp b/point_io.cpp
index fb88301..b9629cc 100644
--- a/point_io.cpp
+++ b/point_io.cpp
@@ -369,6 +369,8 @@ PointSet *colmapReadPointSet(const std::string &filename){
     r->colors.resize(numPoints);
 
     for (size_t i = 0; i < numPoints; i++){
+        readBinary<uint64_t>(reader); // point ID
+
         r->points[i][0] = readBinary<double>(reader);
         r->points[i][1] = readBinary<double>(reader);
         r->points[i][2] = readBinary<double>(reader);

From 8a04a0bdfe840a94d9025d18d95093a2baddeabf Mon Sep 17 00:00:00 2001
From: Piero Toffanin <pt@masseranolabs.com>
Date: Tue, 5 Mar 2024 10:45:41 -0500
Subject: [PATCH 4/5] Fix camToWorld matrix computation, update README

---
 README.md     |  9 ++++-----
 colmap.cpp    | 17 ++++++++++-------
 opensplat.cpp |  6 ------
 3 files changed, 14 insertions(+), 18 deletions(-)

diff --git a/README.md b/README.md
index 2397417..2a9fb91 100644
--- a/README.md
+++ b/README.md
@@ -4,7 +4,7 @@ A free and open source implementation of 3D gaussian splatting written in C++, f
 
 ![OpenSplat](https://github.com/pierotofy/OpenSplat/assets/1951843/3461e0e4-e134-4d6a-8a56-d89d00258e41)
 
-OpenSplat takes camera poses + sparse points and computes a [scene file](https://drive.google.com/file/d/1w-CBxyWNXF3omA8B_IeOsRmSJel3iwyr/view?usp=sharing) (.ply) that can be later imported for viewing, editing and rendering in other [software](https://github.com/MrNeRF/awesome-3D-gaussian-splatting?tab=readme-ov-file#open-source-implementations).
+OpenSplat takes camera poses + sparse points (in [COLMAP](https://colmap.github.io/) or [nerfstudio](https://docs.nerf.studio/quickstart/custom_dataset.html) project format) and computes a [scene file](https://drive.google.com/file/d/1w-CBxyWNXF3omA8B_IeOsRmSJel3iwyr/view?usp=sharing) (.ply) that can be later imported for viewing, editing and rendering in other [software](https://github.com/MrNeRF/awesome-3D-gaussian-splatting?tab=readme-ov-file#open-source-implementations).
 
 Commercial use allowed and encouraged under the terms of the [AGPLv3](https://www.tldrlegal.com/license/gnu-affero-general-public-license-v3-agpl-3-0). ✅
 
@@ -28,7 +28,9 @@ Requirements:
  The software has been tested on Ubuntu 20.04 and Windows. With some changes it could run on macOS (help us by opening a PR?).
 
 ## Build Docker Image
+
 Navigate to the root directory of OpenSplat repo that has Dockerfile and run the following command to build the Docker image:
+
 ```bash
 docker build -t opensplat .
 ```
@@ -60,10 +62,7 @@ Wrote splat.ply
 
 The output `splat.ply` can then be dragged and dropped in one of the many [viewers](https://github.com/MrNeRF/awesome-3D-gaussian-splatting?tab=readme-ov-file#viewers) such as  https://playcanvas.com/viewer. You can also edit/cleanup the scene using https://playcanvas.com/supersplat/editor
 
-To run on your own data, choose the path to an existing [nerfstudio](https://docs.nerf.studio/) project. The project must have sparse points included (random initialization is not supported, see https://github.com/pierotofy/OpenSplat/issues/7). You can generate nerfstudio projects from [COLMAP](https://github.com/colmap/colmap/) by using nerfstudio's `ns-process-data` command: https://docs.nerf.studio/quickstart/custom_dataset.html
-
-
-We have plans to add support for reading COLMAP projects directly in the near future. See https://github.com/pierotofy/OpenSplat/issues/1
+To run on your own data, choose the path to an existing [COLMAP](https://colmap.github.io/) or [nerfstudio](https://docs.nerf.studio/) project. The project must have sparse points included (random initialization is not supported, see https://github.com/pierotofy/OpenSplat/issues/7).
 
 There's several parameters you can tune. To view the full list:
 
diff --git a/colmap.cpp b/colmap.cpp
index 5ed8444..6368841 100644
--- a/colmap.cpp
+++ b/colmap.cpp
@@ -84,16 +84,15 @@ InputData inputDataFromColmap(const std::string &projectRoot){
             readBinary<double>(imgf),
             readBinary<double>(imgf)
         }, torch::kFloat32);
-        torch::Tensor R = quatToRotMat(qVec).transpose(0, 1);
+        torch::Tensor R = quatToRotMat(qVec);
         torch::Tensor T = torch::tensor({
             { readBinary<double>(imgf) },
             { readBinary<double>(imgf) },
             { readBinary<double>(imgf) }
         }, torch::kFloat32);
 
-        // TODO: check cam2world matrix
-        // torch::Tensor Rinv = R.transpose(0, 1);
-        // torch::Tensor Tinv = torch::matmul(-Rinv, T);
+        torch::Tensor Rinv = R.transpose(0, 1);
+        torch::Tensor Tinv = torch::matmul(-Rinv, T);
 
         uint32_t camId = readBinary<uint32_t>(imgf);
 
@@ -110,9 +109,13 @@ InputData inputDataFromColmap(const std::string &projectRoot){
         // TODO: should "images" be an option?
         cam.filePath = (fs::path(projectRoot) / "images" / filePath).string();
 
-        unorientedPoses.index_put_({Slice(i), Slice(None, 3), Slice(None, 3)}, R);
-        unorientedPoses.index_put_({Slice(i), Slice(None, 3), Slice(3, 4)}, T);
-        
+        unorientedPoses[i].index_put_({Slice(None, 3), Slice(None, 3)}, Rinv);
+        unorientedPoses[i].index_put_({Slice(None, 3), Slice(3, 4)}, Tinv);
+        unorientedPoses[i][3][3] = 1.0f;
+
+        // Convert COLMAP's camera CRS (OpenCV) to OpenGL
+        unorientedPoses[i].index_put_({Slice(0, 3), Slice(1,3)}, unorientedPoses[i].index({Slice(0, 3), Slice(1,3)}) * -1.0f);
+
         size_t numPoints2D = readBinary<uint64_t>(imgf);
         for (size_t j = 0; j < numPoints2D; j++){
             readBinary<double>(imgf); // x
diff --git a/opensplat.cpp b/opensplat.cpp
index de4bbc9..234038a 100644
--- a/opensplat.cpp
+++ b/opensplat.cpp
@@ -88,12 +88,6 @@ int main(int argc, char *argv[]){
         InputData inputData = inputDataFromX(projectRoot);
         for (Camera &cam : inputData.cameras){
             cam.loadImage(downScaleFactor);
-            std::cout << cam.fx << " " << cam.fy << " " << 
-                    cam.width << " " << cam.height << std::endl <<
-                    cam.cx << " " << cam.cy << std::endl <<
-                    cam.camToWorld << std::endl;
-                    
-            exit(1);
         }
 
         

From dd89a17581828f8aa563ab5869b7db8dc71ab04d Mon Sep 17 00:00:00 2001
From: Piero Toffanin <pt@masseranolabs.com>
Date: Tue, 5 Mar 2024 10:59:24 -0500
Subject: [PATCH 5/5] Add reading points message

---
 point_io.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/point_io.cpp b/point_io.cpp
index b9629cc..bf91824 100644
--- a/point_io.cpp
+++ b/point_io.cpp
@@ -365,6 +365,8 @@ PointSet *colmapReadPointSet(const std::string &filename){
 
     auto *r = new PointSet();
     size_t numPoints = readBinary<uint64_t>(reader);
+    std::cout << "Reading " << numPoints << " points" << std::endl;
+
     r->points.resize(numPoints);
     r->colors.resize(numPoints);