#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;
}
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()
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent