colmap to megba problem, coordinate systems ? not sure cause it getting wrong results with original datasets too

[chlebaczysko]

okay'ish positions but rotation still off

#include <gflags/gflags.h>

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <map>
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
#include <set>
#include <sstream>
#include <string>
#include <unordered_map>
#include <vector>

#include "algo/lm_algo.h"
#include "edge/base_edge.h"
#include "geo/geo.cuh"
#include "linear_system/schur_LM_linear_system.h"
#include "problem/base_problem.h"
#include "solver/schur_pcg_solver.h"
#include "vertex/base_vertex.h"

using namespace Eigen;

// Define M_PI if not defined
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

// Structs to hold parsed data
struct Camera {
  int id = 0;
  std::string model = "";
  int width = 0;
  int height = 0;
  std::vector<double> params;
  double fx = 0.0;
  double fy = 0.0;
  double cx = 0.0;
  double cy = 0.0;
  double k1 = 0.0;
  double k2 = 0.0;
};
struct Image {
  int id = 0;
  Eigen::Quaterniond quaternion;
  Eigen::Vector3d translation;
  int camera_id = 0;
  std::string name = "";
  std::vector<Eigen::Vector2d> keypoints;
  std::vector<int> point3D_ids;
  int width = 0;
  int height = 0;
  double fx = 0.0;
  double fy = 0.0;
  double cx = 0.0;
  double cy = 0.0;
  double k1 = 0.0;
  double k2 = 0.0;
};

struct Point3D {
  int id = 0;
  Eigen::Vector3d position = Eigen::Vector3d::Zero();
  Eigen::Vector3i color = Eigen::Vector3i::Zero();
  double error = 0.0;
  std::vector<std::pair<int, int>> observations;
};
struct Observation {
  int image_id;
  int point3D_id;
  Eigen::Vector2d keypoint;
  // For sorting
  bool operator<(const Observation& other) const {
    if (point3D_id != other.point3D_id) return point3D_id < other.point3D_id;
    return image_id < other.image_id;
  }
};
// Function prototypes
bool readCameras(const std::string& path, std::vector<Camera>& cameras);
bool readImages(const std::string& path, std::vector<Image>& images,
                const std::vector<Camera>& cameras, int& nameCount);
bool readPoints3D(const std::string& path, std::vector<Point3D>& points3D);
int countPoints3D(const std::vector<Point3D>& points3D);
int countObservations(const std::vector<Point3D>& points3D);
void generateSummary(int nameCount, int numPoints3D, int numObservations,
                     std::ostringstream& summaryOutput);
std::vector<Observation> collectObservations(
    const std::vector<Image>& images, const std::vector<Point3D>& points3D);
void generateObservations(const std::vector<Observation>& observations,
                          std::ostringstream& observationsOutput);
void generateCameraDetails(const std::vector<Image>& images,
                           const std::vector<Camera>& cameras,
                           std::ostringstream& cameraDetailsOutput);
void generatePoints3D(const std::vector<Point3D>& points3D,
                      std::ostringstream& points3DOutput);
void renumberPoint3DIds(std::vector<Observation>& observations);
void saveToFile(const std::string& summaryPath,
                const std::string& observationsPath,
                const std::string& cameraDetailsPath,
                const std::string& points3DPath,
                const std::ostringstream& summaryOutput,
                const std::ostringstream& observationsOutput,
                const std::ostringstream& cameraDetailsOutput,
                const std::ostringstream& points3DOutput);
bool readCameras(const std::string& path, std::vector<Camera>& cameras) {
  std::ifstream fin(path);
  if (!fin) {
    std::cerr << "Error opening file: " << path << std::endl;
    return false;
  }
  std::string line;
  while (std::getline(fin, line)) {
    if (line.empty() || line[0] == '#') continue;
    std::istringstream iss(line);
    Camera cam;
    iss >> cam.id >> cam.model >> cam.width >> cam.height;
    cam.params.assign(std::istream_iterator<double>(iss),
                      std::istream_iterator<double>());
    // Set default values if parameters are missing
    cam.fx = (cam.params.size() > 0) ? cam.params[0] : 1000.0;
    cam.fy = (cam.params.size() > 1) ? cam.params[1] : cam.fx;
    cam.cx = (cam.params.size() > 2) ? cam.params[2] : cam.width / 2.0;
    cam.cy = (cam.params.size() > 3) ? cam.params[3] : cam.height / 2.0;
    cam.k1 = (cam.params.size() > 4) ? cam.params[4] : 0.0;
    cam.k2 = (cam.params.size() > 5) ? cam.params[5] : 0.0;
    cameras.push_back(cam);
  }
  return true;
}

// Funkcja czytająca obrazy z pliku
bool readImages(const std::string& path, std::vector<Image>& images,
                const std::vector<Camera>& cameras, int& nameCount) {
  std::ifstream fin(path);
  if (!fin) {
    std::cerr << "Error opening file: " << path << std::endl;
    return false;
  }
  std::string line;
  std::set<std::string> uniqueNames;
  while (std::getline(fin, line)) {
    if (line.empty() || line[0] == '#') continue;
    std::istringstream iss(line);
    Image img;

    double qw, qx, qy, qz;

    // Read quaternion components
    iss >> img.id >> qw >> qx >> qy >> qz;
    img.quaternion = Eigen::Quaterniond(qw, qx, qy, qz);

    // Store the quaternion in the img object

    iss >> img.translation.x() >> img.translation.y() >> img.translation.z();
    iss >> img.camera_id;
    iss >> img.name;
    uniqueNames.insert(img.name);

    // Przeczytaj kluczowe punkty
    std::string keypoints_line;
    if (std::getline(fin, keypoints_line)) {
      std::istringstream keypoints_iss(keypoints_line);
      double x, y;
      int pt3d_id;
      while (keypoints_iss >> x >> y >> pt3d_id) {
        img.keypoints.emplace_back(x, y);
        img.point3D_ids.push_back(pt3d_id);
      }
    }

    // Pobierz parametry kamery i ustaw domyślne, jeśli to konieczne
    auto it = std::find_if(cameras.begin(), cameras.end(),
                           [camera_id = img.camera_id](const Camera& cam) {
                             return cam.id == camera_id;
                           });
    if (it != cameras.end()) {
      const Camera& cam = *it;
      img.fx = (cam.params.size() > 0) ? cam.params[0] : 1000.0;
      img.fy = (cam.params.size() > 1) ? cam.params[1] : img.fx;
      img.cx = (cam.params.size() > 2) ? cam.params[2]
                                       : 1920;  // Przykład szerokości obrazu
      img.cy = (cam.params.size() > 3) ? cam.params[3]
                                       : 1080;  // Przykład wysokości obrazu
      img.k1 = (cam.params.size() > 4) ? cam.params[4] : 0.0;
      img.k2 = (cam.params.size() > 5) ? cam.params[5] : 0.0;
    } else {
      img.fx = img.fy = img.cx = img.cy = img.k1 = img.k2 = 0.0;
    }

    images.push_back(img);
  }
  nameCount = static_cast<int>(uniqueNames.size());
  return true;
}
bool readPoints3D(const std::string& path, std::vector<Point3D>& points3D) {
  std::ifstream fin(path);
  if (!fin) {
    std::cerr << "Error opening file: " << path << std::endl;
    return false;
  }
  std::string line;
  while (std::getline(fin, line)) {
    if (line.empty() || line[0] == '#') continue;
    std::istringstream iss(line);
    Point3D pt3d;
    iss >> pt3d.id >> pt3d.position.x() >> pt3d.position.y() >>
        pt3d.position.z();
    int image_id, keypoint_index;
    while (iss >> image_id >> keypoint_index) {
      pt3d.observations.emplace_back(image_id, keypoint_index);
    }
    points3D.push_back(pt3d);
  }
  return true;
}
int countPoints3D(const std::vector<Point3D>& points3D) {
  return static_cast<int>(points3D.size());
}
int countObservations(const std::vector<Observation>& observations) {
  return static_cast<int>(observations.size());
}
void generateSummary(int nameCount, int numPoints3D, int numObservations,
                     std::ostringstream& summaryOutput) {
  summaryOutput << nameCount << ' ' << numPoints3D << ' ' << numObservations;
}
std::vector<Observation> collectObservations(
    const std::vector<Image>& images, const std::vector<Point3D>& points3D) {
  std::vector<Observation> observations;
  for (const auto& image : images) {
    for (size_t i = 0; i < image.keypoints.size(); ++i) {
      const auto& pt3D_id = image.point3D_ids[i];
      if (pt3D_id >= 0 && pt3D_id < points3D.size()) {
        observations.push_back({image.id, pt3D_id, image.keypoints[i]});
      }
    }
  }
  return observations;
}

void generateObservations(const std::vector<Observation>& observations,
                          std::ostringstream& observationsOutput,
                          const std::vector<Image>& images) {
  for (const auto& observation : observations) {
    const auto& image = images[observation.image_id];
    // Przesunięcie współrzędnych o cx i cy
    double adjusted_x = observation.keypoint.x();
    double adjusted_y = observation.keypoint.y();

    // Decrement image_id by 1 (jeśli jest to wymagane przez format wyjściowy)
    observationsOutput << (observation.image_id - 1) << ' '
                       << observation.point3D_id << ' ' << adjusted_x << ' '
                       << adjusted_y << '\n';
  }
}

using namespace cv;
using namespace std;
// Define the function to convert Rodrigues' rotation vector to a rotation
// matrix
Eigen::Matrix3d fromRodrigues(const Eigen::Vector3d& x) {
  double theta2 = x.squaredNorm();
  if (theta2 > std::numeric_limits<double>::epsilon()) {
    double angle = x.norm();
    Eigen::Vector3d axis = x.normalized();
    Eigen::AngleAxisd rotation(angle, axis);
    return rotation.toRotationMatrix();
  } else {
    // Taylor series approximation from ceres-solver
    Eigen::Matrix3d rotation;
    rotation << 1.0, x.z(), -x.y(), -x.z(), 1.0, x.x(), x.y(), -x.x(), 1.0;
    return rotation;
  }
}

// Function to convert rotation matrix to quaternion
Eigen::Quaternion<double> rotationMatrixToQuaternion(
    const Eigen::Matrix3d& rotation) {
  return Eigen::Quaternion<double>(rotation);
}

Vector3d QuaternionToRodrigues(const Quaterniond& quat) {
  // Krok 1: Konwertuj kwaternion na macierz rotacji
  Matrix3d rotationMatrix = quat.toRotationMatrix();

  // Krok 2: Oblicz wektor Rodrigueza z macierzy rotacji
  // Wektor Rodrigueza jest definiowany przez cosinus kąta rotacji oraz oś
  // rotacji
  Vector3d rodrigues;

  double angle = acos((rotationMatrix.trace() - 1) / 2.0);
  if (angle != 0) {
    double sin_angle = sin(angle);
    rodrigues(0) =
        (rotationMatrix(2, 1) - rotationMatrix(1, 2)) / (2 * sin_angle);
    rodrigues(1) =
        (rotationMatrix(0, 2) - rotationMatrix(2, 0)) / (2 * sin_angle);
    rodrigues(2) =
        (rotationMatrix(1, 0) - rotationMatrix(0, 1)) / (2 * sin_angle);
    rodrigues *= angle;
  } else {
    rodrigues << 0, 0, 0;
  }

  return rodrigues;
}
void cvMatToEigen(const cv::Mat& cvMat, Eigen::Matrix3d& eigenMat) {
  assert(cvMat.rows == 3 && cvMat.cols == 3 && cvMat.type() == CV_64F);
  // Kopiuj wartości z cv::Mat do Eigen::Matrix3d
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      eigenMat(i, j) = cvMat.at<double>(i, j);
    }
  }
}
// Function to convert Eigen quaternion to (w, x, y, z) format
void quaternionToWXYZ(const Eigen::Quaterniond& quat, double& w, double& x,
                      double& y, double& z) {
  w = quat.w();
  x = quat.x();
  y = quat.y();
  z = quat.z();
}

template <typename T>
T clamp(T value, T low, T high) {
  return (value < low) ? low : (value > high) ? high : value;
}


// Function to generate camera details
void generateCameraDetails(const vector<Image>& images,
                           const vector<Camera>& cameras,
                           ostringstream& cameraDetailsOutput) {
  unordered_map<int, string> imageIdToName;
  for (const auto& image : images) {
    imageIdToName[image.id] = image.name;
  }

  unordered_map<int, int> idMap;
  int newId = 0;
  for (const auto& image : images) {
    idMap[image.id] = newId++;
  }

  unordered_map<int, Camera> cameraMap;
  for (const auto& cam : cameras) {
    cameraMap[cam.id] = cam;
  }

  for (const auto& image : images) {
    int newImageId = idMap[image.id];
    const auto& cam = cameraMap[image.camera_id];

  
    Quaterniond quat(image.quaternion);
    Matrix3d rotationMatrix = quat.toRotationMatrix();

  
    Matrix4d w2c = Matrix4d::Identity();
    w2c.block<3, 3>(0, 0) = rotationMatrix;
    w2c.block<3, 1>(0, 3) = image.translation;

   
    Matrix4d c2w = w2c.inverse();
  
    Vector3d translationFromMatrix = c2w.block<3, 1>(0, 3);
    Matrix3d rotationMatrixWorld = c2w.block<3, 3>(0, 0);

    Vector3d rodrigues = QuaternionToRodrigues(quat);


    cameraDetailsOutput << rodrigues[0] << '\n'
                        << rodrigues[1] << '\n'
                        << rodrigues[2] << '\n'
                        << translationFromMatrix[0] << '\n'
                        << translationFromMatrix[1] << '\n'
                        << translationFromMatrix[2] << '\n'
                        << (cam.fx + cam.fy) / 2.0 << '\n'
                        << cam.k1 << '\n'
                        << cam.k2 << '\n';
  }
}

// Funkcja generująca punkty 3D
void generatePoints3D(const std::vector<Point3D>& points3D,
                      std::ostringstream& points3DOutput) {
  for (const auto& point3D : points3D) {
    Eigen::Vector3d position = point3D.position;

    points3DOutput << position.x() << '\n'
                   << position.y() << '\n'
                   << position.z() << '\n';
  }
}
void saveToFile(const std::string& outputPath,
                const std::ostringstream& summaryOutput,
                const std::ostringstream& observationsOutput,
                const std::ostringstream& cameraDetailsOutput,
                const std::ostringstream& points3DOutput) {
  // Open the output file
  std::ofstream fout(outputPath);
  if (!fout) {
    std::cerr << "Error opening file for writing: " << outputPath << std::endl;
    return;
  }

  // Write summary
  fout << summaryOutput.str();
  fout << std::endl;

  // Write observations
  fout << observationsOutput.str();

  // Write camera details
  fout << cameraDetailsOutput.str();

  // Write points3D
  fout << points3DOutput.str();
  fout.close();
}
void renumberPoint3DIds(std::vector<Observation>& observations) {
  std::unordered_map<int, int> oldToNewIdMap;
  int newId = 0;
  for (const auto& obs : observations) {
    if (oldToNewIdMap.find(obs.point3D_id) == oldToNewIdMap.end()) {
      oldToNewIdMap[obs.point3D_id] = newId++;
    }
  }
  for (auto& obs : observations) {
    obs.point3D_id = oldToNewIdMap[obs.point3D_id];
  }
}
void saveToFile(const std::string& summaryPath,
                const std::string& observationsPath,
                const std::string& cameraDetailsPath,
                const std::string& points3DPath,
                const std::ostringstream& summaryOutput,
                const std::ostringstream& observationsOutput,
                const std::ostringstream& cameraDetailsOutput,
                const std::ostringstream& points3DOutput) {
  // Save summary
  std::ofstream summaryFile(summaryPath);
  if (summaryFile) {
    summaryFile << summaryOutput.str();
    summaryFile.close();
  } else {
    std::cerr << "Error opening file for writing: " << summaryPath << std::endl;
  }
  // Save observations
  std::ofstream observationsFile(observationsPath);
  if (observationsFile) {
    observationsFile << observationsOutput.str();
    observationsFile.close();
  } else {
    std::cerr << "Error opening file for writing: " << observationsPath
              << std::endl;
  }
  // Save camera details
  std::ofstream cameraDetailsFile(cameraDetailsPath);
  if (cameraDetailsFile) {
    cameraDetailsFile << cameraDetailsOutput.str();
    cameraDetailsFile.close();
  } else {
    std::cerr << "Error opening file for writing: " << cameraDetailsPath
              << std::endl;
  }
  // Save points3D
  std::ofstream points3DFile(points3DPath);
  if (points3DFile) {
    points3DFile << points3DOutput.str();
    points3DFile.close();
  } else {
    std::cerr << "Error opening file for writing: " << points3DPath
              << std::endl;
  }
}
template <typename T>
class BAL_Edge : public MegBA::BaseEdge<T> {
 public:
  MegBA::JVD<T> forward() override {
    using MappedJVD = Eigen::Map<const MegBA::geo::JVD<T>>;
    const auto& Vertices = this->getVertices();
    MappedJVD angle_axisd{&Vertices[0].getEstimation()(0, 0), 3, 1};
    MappedJVD t{&Vertices[0].getEstimation()(3, 0), 3, 1};
    MappedJVD intrinsics{&Vertices[0].getEstimation()(6, 0), 3, 1};
    const auto& point_xyz = Vertices[1].getEstimation();
    const auto& obs_uv = this->getMeasurement();
    auto R = MegBA::geo::AngleAxisToRotationKernelMatrix(angle_axisd);
    Eigen::Matrix<MegBA::JetVector<T>, 3, 1> re_projection = R * point_xyz + t;
    re_projection = -re_projection / re_projection(2);
    // f, k1, k2 = intrinsics
    auto fr = MegBA::geo::RadialDistortion(re_projection, intrinsics);
    MegBA::JVD<T> error = fr * re_projection.head(2) - obs_uv;
    return error;
  }
};

namespace {
template <typename Derived>
bool writeVector(std::ostream& os, const Eigen::DenseBase<Derived>& b) {
  for (int i = 0; i < b.size(); i++) os << b(i) << " ";
  return os.good();
}
template <typename Derived>
bool readVector(std::istream& is, Eigen::DenseBase<Derived>& b) {
  for (int i = 0; i < b.size() && is.good(); i++) is >> b(i);
  return is.good() || is.eof();
}
}  // namespace
DEFINE_int32(world_size, 1, "World size");
DEFINE_string(out_path, "output.txt", "Path to save the results");
DEFINE_string(path, "", "Path to your dataset");
DEFINE_int32(max_iter, 20, "LM solve iteration");
DEFINE_int32(solver_max_iter, 50, "Linear solver iteration");
DEFINE_double(solver_tol, 10., "The tolerance of the linear solver");
DEFINE_double(solver_refuse_ratio, 1., "The refuse ratio of the linear solver");
DEFINE_double(tau, 1., "Initial trust region");
DEFINE_double(epsilon1, 1., "Parameter of LM");
DEFINE_double(epsilon2, 1e-10, "Parameter of LM");
int main(int argc, char* argv[]) {
  GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
  std::cout << "solving " << FLAGS_path << ", world_size: " << FLAGS_world_size
            << ", max iter: " << FLAGS_max_iter
            << ", solver_tol: " << FLAGS_solver_tol
            << ", solver_refuse_ratio: " << FLAGS_solver_refuse_ratio
            << ", solver_max_iter: " << FLAGS_solver_max_iter
            << ", tau: " << FLAGS_tau << ", epsilon1: " << FLAGS_epsilon1
            << ", epsilon2: " << FLAGS_epsilon2 << std::endl;
  typedef double T;
  const std::string cameras_path = "cameras.txt";
  const std::string images_path = "images.txt";
  const std::string points3D_path = "points3D.txt";
  std::vector<Camera> cameras;
  std::vector<Image> images;
  std::vector<Point3D> points3D;
  int nameCount = 0;

  if (!readCameras(cameras_path, cameras)) {
    std::cerr << "Error reading cameras file" << std::endl;
    return 1;
  }
  if (!readImages(images_path, images, cameras, nameCount)) {
    std::cerr << "Error reading images file" << std::endl;
    return 1;
  }
  if (!readPoints3D(points3D_path, points3D)) {
    std::cerr << "Error reading points3D file" << std::endl;
    return 1;
  }

  auto observations = collectObservations(images, points3D);
  std::sort(observations.begin(), observations.end());

  // Renumber the point3D_ids
  renumberPoint3DIds(observations);

  int numPoints3D = countPoints3D(points3D);
  int numObservations = countObservations(observations);

  std::ostringstream summaryOutput;
  std::ostringstream observationsOutput;
  std::ostringstream cameraDetailsOutput;
  std::ostringstream points3DOutput;

  generateSummary(nameCount, numPoints3D, numObservations, summaryOutput);
  generateObservations(observations, observationsOutput, images);
  generateCameraDetails(images, cameras, cameraDetailsOutput);
  generatePoints3D(points3D, points3DOutput);

  // Save all data to a single file
  saveToFile(FLAGS_out_path, summaryOutput, observationsOutput,
             cameraDetailsOutput, points3DOutput);
  std::ifstream fin(FLAGS_path);
  if (!fin) {
    std::cerr << "Error opening input file: " << FLAGS_path << std::endl;
    return 1;
  }
  fin >> nameCount;
  fin >> numPoints3D;
  fin >> numObservations;
  MegBA::ProblemOption problemOption{};
  problemOption.nItem = numObservations;
  problemOption.N = 12;
  for (int i = 0; i < FLAGS_world_size; ++i) {
    problemOption.deviceUsed.push_back(i);
  }
  MegBA::SolverOption solverOption{};
  solverOption.solverOptionPCG.maxIter = FLAGS_solver_max_iter;
  solverOption.solverOptionPCG.tol = FLAGS_solver_tol;
  solverOption.solverOptionPCG.refuseRatio = FLAGS_solver_refuse_ratio;
  MegBA::AlgoOption algoOption{};
  algoOption.algoOptionLM.maxIter = FLAGS_max_iter;
  algoOption.algoOptionLM.initialRegion = FLAGS_tau;
  algoOption.algoOptionLM.epsilon1 = FLAGS_epsilon1;
  algoOption.algoOptionLM.epsilon2 = FLAGS_epsilon2;
  std::unique_ptr<MegBA::BaseAlgo<T>> algo{
      new MegBA::LMAlgo<T>{problemOption, algoOption}};
  std::unique_ptr<MegBA::BaseSolver<T>> solver{
      new MegBA::SchurPCGSolver<T>{problemOption, solverOption}};
  std::unique_ptr<MegBA::BaseLinearSystem<T>> linearSystem{
      new MegBA::SchurLMLinearSystem<T>{problemOption, std::move(solver)}};
  MegBA::BaseProblem<T> problem{problemOption, std::move(algo),
                                std::move(linearSystem)};
  std::vector<std::tuple<int, int, Eigen::Matrix<T, 2, 1>>> edge;
  std::vector<std::tuple<int, Eigen::Matrix<T, 9, 1>>> camera_vertices;
  std::vector<std::tuple<int, Eigen::Matrix<T, 3, 1>>> point_vertices;
  int counter = 0;
  // Read edges
  while (!fin.eof()) {
    if (counter < numObservations) {
      int idx1, idx2;
      fin >> idx1 >> idx2;
      idx2 += nameCount;
      Eigen::Matrix<T, 2, 1> observations;
      readVector(fin, observations);
      edge.emplace_back(idx1, idx2, std::move(observations));
    } else {
      break;
    }
    counter++;
  }
  // Read vertices and modify camera orientation
  counter = 0;
  while (!fin.eof()) {
    int idx = counter;
    if (counter < nameCount) {
      Eigen::Matrix<T, 9, 1> camera_vector9;
      readVector(fin, camera_vector9);
      camera_vertices.emplace_back(idx, std::move(camera_vector9));
    } else {
      Eigen::Matrix<T, 3, 1> point_Vector3;
      readVector(fin, point_Vector3);
      point_vertices.emplace_back(idx, std::move(point_Vector3));
    }
    counter++;
    if (!fin.good()) break;
  }
  for (int n = 0; n < nameCount; ++n) {
    problem.appendVertex(std::get<0>(camera_vertices[n]),
                         new MegBA::CameraVertex<T>());
    problem.getVertex(std::get<0>(camera_vertices[n]))
        .setEstimation(std::get<1>(std::move(camera_vertices[n])));
  }
  for (int n = 0; n < numPoints3D; ++n) {
    problem.appendVertex(std::get<0>(point_vertices[n]),
                         new MegBA::PointVertex<T>());
    problem.getVertex(std::get<0>(point_vertices[n]))
        .setEstimation(std::get<1>(std::move(point_vertices[n])));
  }
  for (int j = 0; j < numObservations; ++j) {
    auto edgePtr = new BAL_Edge<T>;
    edgePtr->appendVertex(&problem.getVertex(std::get<0>(edge[j])));
    edgePtr->appendVertex(&problem.getVertex(std::get<1>(edge[j])));
    edgePtr->setMeasurement(std::get<2>(std::move(edge[j])));
    problem.appendEdge(*edgePtr);
  }
  problem.solve();
  // Write results to output file
  std::ofstream fout(FLAGS_out_path);
  if (!fout) {
    std::cerr << "Error opening output file: " << FLAGS_out_path << std::endl;
    return 1;
  }
  // Write number of cameras, points, and observations in one line
  // Write number of cameras, points, and observations in one line
  fout << nameCount << " " << numPoints3D << " " << numObservations
       << std::endl;

  // Write observations (edges) first
  for (const auto& e : edge) {
    fout << std::get<0>(e) << " " << std::get<1>(e) << " ";
    writeVector(fout, std::get<2>(e));
    fout << std::endl;
  }

    for (const auto& camera_vertex : camera_vertices) {
    Eigen::Matrix<double, 9, 1> camera_vector = std::get<1>(camera_vertex);

    Eigen::Vector3d rvec(camera_vector(0), camera_vector(1), camera_vector(2));
    Eigen::Vector3d translation(camera_vector(3), camera_vector(4),
                                camera_vector(5));
    Eigen::Vector3d intrinsics(camera_vector(6), camera_vector(7),
                               camera_vector(8));

    // Convert Rodrigues' vector to rotation matrix
    Eigen::Matrix3d rotationMatrix = fromRodrigues(rvec);

    // Convert rotation matrix to quaternion
    Eigen::Quaternion<double> quaternion =
        rotationMatrixToQuaternion(rotationMatrix);

    // Extract quaternion components
    double w = quaternion.w();
    double x = quaternion.x();
    double y = quaternion.y();
    double z = quaternion.z();

    // Write quaternion and other parameters to the file
    fout << w << " " << x << " " << y << " " << z << " ";
    fout << translation.x() << " " << translation.y() << " " << translation.z()
         << " ";
    fout << intrinsics.x() << " " << intrinsics.y() << " " << intrinsics.z()
         << std::endl;
  }

  // Write point vertices
  for (const auto& point_vertex : point_vertices) {
    // Comment out or remove the ID part if not needed
    // fout << std::get<0>(point_vertex) << " ";
    writeVector(fout, std::get<1>(point_vertex));
    fout << std::endl;
  }

  // Clean up and close the file
  gflags::ShutDownCommandLineFlags();
  fout.close();
  return 0;
}

im not sure what am i doing wrong

import numpy as np
import open3d as o3d
from scipy.spatial.transform import Rotation as R

def qvec2rotmat(qvec: np.ndarray) -> np.ndarray:
    """Convert a quaternion vector to a rotation matrix."""
    q0, q1, q2, q3 = qvec
    return np.array([
        [1 - 2 * (q2**2 + q3**2), 2 * (q1 * q2 - q0 * q3), 2 * (q1 * q3 + q0 * q2)],
        [2 * (q1 * q2 + q0 * q3), 1 - 2 * (q1**2 + q3**2), 2 * (q2 * q3 - q0 * q1)],
        [2 * (q1 * q3 - q0 * q2), 2 * (q2 * q3 + q0 * q1), 1 - 2 * (q1**2 + q2**2)]
    ])

def read_bal_data(file_name):
    """
    Reads BAL data from a text file.
    """
    with open(file_name, "r") as file:
        n_cameras, n_points, n_observations = map(int, file.readline().split())

        points_2d = np.empty((n_observations, 2))
        camera_indices = np.empty(n_observations, dtype=int)
        point_indices = np.empty(n_observations, dtype=int)

        points_3d_dict = {}

        for i in range(n_observations):
            try:
                camera_index, point_index, x, y = file.readline().split()
                camera_indices[i] = int(camera_index)
                point_indices[i] = int(point_index)
                points_2d[i] = [float(x), float(y)]
            except ValueError:
                print(f"Warning: Malformed observation line at index {i}")

        camera_params = np.empty((n_cameras, 10))  # 4 quaternion values + 3 translation values + 1 focal + 2 distortion coefficients

        for i in range(n_cameras):
            try:
                line = file.readline().split()
                quaternion = list(map(float, line[0:4]))  # Quaternion (WXYZ)
                translation = list(map(float, line[4:7]))  # Translation
                focal = float(line[7])
                k1 = float(line[8])
                k2 = float(line[9])
                camera_params[i] = quaternion + translation + [focal, k1, k2]
            except ValueError:
                print(f"Warning: Malformed camera parameters line at index {i}")

        for i in range(n_points):
            try:
                line = file.readline().split()
                point_coords = list(map(float, line[0:3]))
                if len(point_coords) == 2:
                    point_coords.append(0.0)
                points_3d_dict[i] = point_coords
            except ValueError:
                print(f"Warning: Malformed 3D point line at index {i}")

        points_3d = np.full((n_points, 3), np.nan)
        for index, coords in points_3d_dict.items():
            points_3d[index] = coords

    return camera_params, points_3d, camera_indices, point_indices, points_2d

def create_camera_frustum():
    """
    Creates a frustum to represent the camera's view.
    """
    # Define frustum corners in camera space
    frustum_points = np.array([
        [0, 0, 0],  # Apex
        [0.05, 0.05, 0.1],  # Near upper right
        [-0.05, 0.05, 0.1],  # Near upper left
        [-0.05, -0.05, 0.1],  # Near lower left
        [0.05, -0.05, 0.1],  # Near lower right
        [0.05, 0.05, 0.2],  # Far upper right
        [-0.05, 0.05, 0.2],  # Far upper left
        [-0.05, -0.05, 0.2],  # Far lower left
        [0.05, -0.05, 0.2],  # Far lower right
    ])

    # Define frustum lines
    frustum_lines = np.array([
        [0, 1], [0, 2], [0, 3], [0, 4],  # Apex to near plane
        [1, 2], [2, 3], [3, 4], [4, 1],  # Near plane
        [5, 6], [6, 7], [7, 8], [8, 5],  # Far plane
        [1, 5], [2, 6], [3, 7], [4, 8]   # Near to far plane
    ])

    lines = o3d.geometry.LineSet()
    lines.points = o3d.utility.Vector3dVector(frustum_points)
    lines.lines = o3d.utility.Vector2iVector(frustum_lines)

    # Set color
    colors = np.array([[0.5, 0.5, 0.5] for _ in frustum_lines])
    lines.colors = o3d.utility.Vector3dVector(colors)

    return lines

def create_global_axes():
    """
    Creates global axes as a reference.
    """
    lines = o3d.geometry.LineSet()

    axis_length = 0.1
    points = np.array([
        [0, 0, 0],  # Origin
        [axis_length, 0, 0],  # X-axis
        [0, axis_length, 0],  # Y-axis
        [0, 0, axis_length]   # Z-axis
    ])

    lines.lines = o3d.utility.Vector2iVector([
        [0, 1],  # X-axis
        [0, 2],  # Y-axis
        [0, 3]   # Z-axis
    ])

    lines.points = o3d.utility.Vector3dVector(points)

    colors = np.array([
        [1, 0, 0],  # X-axis color (red)
        [0, 1, 0],  # Y-axis color (green)
        [0, 0, 1]   # Z-axis color (blue)
    ])
    lines.colors = o3d.utility.Vector3dVector(colors)

    return lines

def visualize_scene(camera_params, points_3d):
    vis = o3d.visualization.Visualizer()
    vis.create_window()

    # Add the 3D points
    point_cloud = o3d.geometry.PointCloud()
    point_cloud.points = o3d.utility.Vector3dVector(points_3d)
    vis.add_geometry(point_cloud)

    # Add global axes
    global_axes = create_global_axes()
    vis.add_geometry(global_axes)

    for camera_id, param in enumerate(camera_params):
        quaternion = param[:4]  # Quaternion (WXYZ)
        translation = param[4:7]  # Translation

        # Convert quaternion to rotation matrix
        rot = R.from_quat(quaternion)  # WXYZ format for conversion
        rotation_matrix = rot.as_matrix()

        frustum = create_camera_frustum()
        frustum.rotate(rotation_matrix, center=(0, 0, 0))
        frustum.translate(translation)

        vis.add_geometry(frustum)

    vis.run()
    vis.destroy_window()

def main():
    # Replace 'path/to/your/data.txt' with the actual path to your BAL file
    file_name = "output.txt"

    # Read BAL data
    camera_params, points_3d, _, _, _ = read_bal_data(file_name)
    
    # Visualize the scene
    visualize_scene(camera_params, points_3d)

if __name__ == "__main__":
    main()