Add files via upload

BAProblem.h

@@ -0,0 +1,74 @@
+/**
+* This file is part of MT-BA.
+*
+* Copyright 2018 Cao Lin.
+* Developed by Cao Lin ,
+* If you use this code, please cite the respective publications as
+* listed on the above website.
+*
+* MT-BA is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* MT-BA is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with MT-BA. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#pragma once
+#include "matrix_equation.h"
+
+#include <vector>
+#include <array>
+using namespace std;
+
+typedef array<double, 6> baseCamera;
+typedef array<double, 3> point;
+typedef struct _observer{ double u; double v; int cam; int pt; } observer;
+typedef array<double, 9> balCamera;
+
+class BASolver
+{
+public:
+	BASolver(double* _K);
+	~BASolver() {}
+	void addCamera(baseCamera& _mcamera) { cameras.push_back(_mcamera); }//SE3d
+	void addPoint(point& _mpoint) { points.push_back(_mpoint); }
+	void addObserver(observer& _mobserver) { observers.push_back(_mobserver); }
+	//Levenberg¨CMarquardt Method
+	void solveProblem(const int iteration);
+protected:
+	void sortObservers();
+	//return value : updata matrix
+	clMat* getSolveMatrix(double& cost, double& Q_down);
+	//we will use seven steps to get the resule. 
+	//step 1: calculating the inverse of H22
+	clMat* getH22Inverse(const clMat* H22);
+	//step 2: H12 * H22.inverse, we named the result T
+	clMat* getMatrixT(const clMat* H12, const clMat* H22inverse);
+	//step 3: add T * b2 to b1
+	void getNewb1(const clMat* T, const clMat* b2, clMat* b1);
+	//step 4: get Matrix A = T * H12.transpose
+	clMat* getMatrixA(const clMat* T, const clMat* H12);
+	//step 5: solve the equivalent (H11 - A) * x1 = b1
+	clMat* getMatrix_x1(const clMat* A, const clMat* H11, const clMat* b1);
+	//step 6: b2 -= H12.transpose * x1  
+	void getNewb2(const clMat* H12, const clMat* x1, clMat* b2);
+	//step 7: x2 = H22.inverse * b2
+	clMat* getMatrix_x2(const clMat* H22inverse, const clMat* b2);
+	//compute update for every camera and point
+	void computeUpdate(clMat* update, double cost, double Q_down, int v);
+	//use OpenCV to show the structure of the Matrix mat
+	void showMatrixStructure(clMat& mat);
+	vector<baseCamera> cameras;
+	vector<point> points;
+	vector<observer> observers;
+	double mu;
+	array<double, 4> K;
+
+};

main.cpp

@@ -0,0 +1,61 @@
+#include "BAProblem.h"
+#include <fstream>
+#include <string>
+#include <sophus\se3.hpp>
+
+#include <thread>
+
+using std::ifstream;
+using std::string;
+
+string p3d_file = "./p3d.txt";
+string p2d_file = "./p2d.txt";
+
+int main(int argc, char** argv)
+{
+	Sophus::Vector6d T = Sophus::SE3d().log();
+
+	double K[4] = { 520.9, 521.0, 325.1, 249.7 };
+	BASolver solver(K);
+	baseCamera camera = {T[0], T[1], T[2], T[3], T[4], T[5] };
+	solver.addCamera(camera);
+
+	ifstream inp2p(p2d_file);
+
+	if (!inp2p.is_open()) {
+		cout << "can not find inp2p\n";
+		return 1;
+	}
+	int index = 0;
+	while (!inp2p.eof()) {
+		observer edge;
+		inp2p >> edge.u >> edge.v;
+		edge.cam = 0;
+		edge.pt = index;
+		solver.addObserver(edge);
+		++index;
+	}
+	inp2p.close();
+
+	ifstream inp3p(p3d_file);
+	if (!inp3p.is_open()) {
+		cout << "can not find inp3p\n";
+		return 1;
+	}
+	double fx = 520.9 * 520.9;
+	double fy = 521.0 * 521.0;
+	while (!inp3p.eof()) {
+		point pt;
+		inp3p >> pt[0] >> pt[1] >> pt[2];
+
+		solver.addPoint(pt);
+	}
+	inp3p.close();
+
+	solver.solveProblem(100);
+
+#ifdef _MSC_VER
+	system("pause");
+#endif
+}
+

matrix.cpp

@@ -0,0 +1,114 @@
+/**
+* This file is part of MT-BA.
+*
+* Copyright 2018 Cao Lin.
+* Developed by Cao Lin ,
+* If you use this code, please cite the respective publications as
+* listed on the above website.
+*
+* MT-BA is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* MT-BA is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with MT-BA. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "matrix.h"
+#include <iomanip>
+#include <exception>
+
+clMat::clMat(int _row, int _col) : row(_row), col(_col)
+{
+	if (row < 1) {
+		std::cerr << "row is lower than 1 in " << __FILE__ << " line : " << __LINE__ << endl;
+		throw std::runtime_error("row is lower than 1");
+	}
+	if (col < 1) {
+		std::cerr << "col is lower than 1 in " << __FILE__ << " line : " << __LINE__ << endl;
+		throw std::runtime_error("col is lower than 1");
+	}
+
+	data = new double*[row];
+	data[0] = new double[row * col]();
+
+	for (int i = 1; i < row; ++i)
+	{
+		data[i] = data[0] + i * col;
+	}
+}
+
+clMat::~clMat()
+{
+	delete[]data[0];
+	for (int i = 0; i < row; ++i)
+		data[i] = nullptr;
+	delete[]data;
+	data = nullptr;
+}
+
+void clMat::setData(double* _data)
+{
+#ifdef _MSC_VER
+	memcpy_s(data[0], sizeof(double) * row * col, _data, sizeof(double) * row * col);
+#else
+	memcpy(data[0], _data, sizeof(double) * row * col);
+#endif // check the IDE
+
+}
+
+void clMat::resize(int _row, int _col)
+{
+	delete[]data[0];
+	delete[]data;
+	data = nullptr;
+	row = _row;
+	col = _col;
+
+	if (row < 1) {
+		std::cerr << "row is lower than 1 in " << __FILE__ << " line : " << __LINE__ << endl;
+		throw std::runtime_error("row is lower than 1");
+	}
+	if (col < 1) {
+		std::cerr << "col is lower than 1 in " << __FILE__ << " line : " << __LINE__ << endl;
+		throw std::runtime_error("col is lower than 1");
+	}
+
+	data = new double*[row];
+	data[0] = new double[row * col]();
+
+	for (int i = 1; i < row; ++i)
+	{
+		data[i] = data[0] + i * col;
+	}
+}
+
+void clMat::output(std::ostream& out)const
+{
+	for (int i = 0; i < row; ++i) {
+		for (int j = 0; j < col; ++j) {
+			out << std::setw(7) << data[i][j] << "  ";
+		}
+		out << endl;
+	}
+}
+
+std::ostream& operator << (std::ostream& out, const clMat& x)
+{
+	x.output(out);
+	return out;
+}
+
+clMat clMat::Identity(int _size)
+{
+	clMat* I = new clMat(_size, _size);
+	for (int i = 0; i < _size; ++i)
+		I->data[i][i] = 1;
+	return *I;
+}

matrix.h

@@ -0,0 +1,46 @@
+/**
+* This file is part of MT-BA.
+*
+* Copyright 2018 Cao Lin.
+* Developed by Cao Lin ,
+* If you use this code, please cite the respective publications as
+* listed on the above website.
+*
+* MT-BA is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* MT-BA is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with MT-BA. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#pragma once
+#include <iostream>
+using std::endl;
+using std::cout;
+
+class clMat
+{
+public:
+	clMat(int _row, int _col);
+	~clMat();
+	void setData(double* _data);
+	void resize(int _row, int _col);
+	int getRow()const { return row; }
+	int getCol()const { return col; }
+	void output(std::ostream& out)const;
+	double **data;
+	static clMat Identity(int _size);
+private:
+	int row;
+	int col;
+	clMat() = delete;
+};
+
+std::ostream& operator << (std::ostream& out, const clMat& x);

matrix_equation.cpp

@@ -0,0 +1,80 @@
+/**
+* This file is part of MT-BA.
+*
+* Copyright 2018 Cao Lin.
+* Developed by Cao Lin ,
+* If you use this code, please cite the respective publications as
+* listed on the above website.
+*
+* MT-BA is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* MT-BA is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with MT-BA. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "matrix_equation.h"
+
+void matrix_joint(const clMat& H, const clMat& b, clMat& des)
+{
+	des.resize(H.getRow(), H.getCol() + b.getCol());
+	int row = des.getRow();
+	int h_col = H.getCol();
+	int b_col = b.getCol();
+	for (int i = 0; i < row; ++i)
+	{
+#ifdef _MSC_VER
+
+		for (int j = 0; j < h_col; ++j)
+			des.data[i][j] = H.data[i][j];
+		for (int j = 0; j < b_col; ++j)
+			des.data[i][j + h_col] = b.data[i][j];
+
+#else
+		memcpy(des.data[0] + i * (h_col + b_col), H.data[0] + i * h_col, sizeof(double) * row * h_col);
+		memcpy(des.data[0] + i * (h_col + b_col) + h_col,
+			b.data[0] + i * b_col,
+			sizeof(double) * row * b_col);
+#endif
+	}
+}
+
+void first_step(clMat& des)
+{
+	int N = des.getRow();
+	int M = des.getCol();
+	for (int i = 0; i < N; ++i)
+	{
+		//第一行
+		for (int c = i + 1; c < M; ++c)
+			des.data[i][c] /= des.data[i][i];
+		//后面每一行
+		for (int r = i + 1; r < N; ++r)
+		{
+			for (int c = i + 1; c < M; ++c)
+			{
+				if (des.data[r][i]) {
+					des.data[r][c] /= des.data[r][i];
+					des.data[r][c] -= des.data[i][c];
+				}
+			}
+		}
+	}
+	for (int r = N - 2; r > -1; --r)
+	{//被求解的矩阵
+		for (int c = N; c < M; ++c)
+		{
+			for (int i = N - 1; i > r; --i)
+			{
+				des.data[r][c] -= des.data[i][c] * des.data[r][i];
+			}			
+		}
+	}
+}