Atsumi Toda
/
UAV_Logger1
UAVの姿勢推定に使用するプログラム。
Diff: Matrix/Matrix.cpp
- Revision:
- 4:21a356ae0747
- Parent:
- 3:3fa7882a5fd0
--- a/Matrix/Matrix.cpp Wed Jul 24 12:00:01 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,388 +0,0 @@ -#include "myConstants.h" -#include "Matrix.h" - - - -Matrix::Matrix(int row, int col) : row(row), col(col), components(0) { - components = new float[row*col]; - if (!components) error("Memory Allocation Error"); - for(int i=0; i<row*col; i++) components[i] = 0.0f; - if (row == col) { - for (int i = 0; i < row; i++) { - components[i * col + i] = 1.0f; - } - } -} - -Matrix::Matrix(int row, int col, float* comps) : row(row), col(col), components(0) { - components = new float[row*col]; - if (!components) error("Memory Allocation Error"); - memcpy(components, comps, sizeof(float)*row*col); -} - - -Matrix::~Matrix() { - delete[] components; -} - -Matrix::Matrix(const Matrix& m) : row(m.row), col(m.col), components(0) { - components = new float[row*col]; - if (!components) error("Memory Allocation Error"); - memcpy(components, m.GetpComponents(), sizeof(float)*row*col); -} - -Matrix Matrix::operator-() const{ - Matrix retMat(*this); - - for (int i = 0; i < row * col; i++) { - retMat.components[i] = - this->components[i]; - } - - return retMat; -} - -Matrix& Matrix::operator=(const Matrix& m) { - if (this == &m) return *this; - row = m.row; - col = m.col; - delete[] components; - components = new float[row*col]; - if (!components) error("Memory Allocation Error"); - memcpy(components, m.GetpComponents(), sizeof(float)*row*col); - - return *this; -} - -Matrix& Matrix::operator+=(const Matrix& m) { - if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention"); - - for (int i = 0; i < row; i++) { - for (int j = 0; j < col; j++) { - components[i * col + j] += m.components[i * col + j]; - } - } - - this->CleanUp(); - - return *this; -} - -Matrix& Matrix::operator-=(const Matrix& m) { - if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention"); - - for (int i = 0; i < row; i++) { - for (int j = 0; j < col; j++) { - components[i * col + j] -= m.components[i * col + j]; - } - } - - this->CleanUp(); - - return *this; -} -/* -Matrix& Matrix::operator*=(const Matrix& m) { - if (col != m.GetRow()) error("Irregular Dimention"); - Matrix temp = Matrix(*this); - - col = m.GetCol(); - delete[] components; - components = new float[row*col]; - - for (int i = 0; i < row; i++) { - for (int j = 0; j < col; j++) { - components[i*col + j] = 0.0f; - for (int k = 0; k < m.GetRow(); k++) { - components[i * col + j] += temp.components[i * col + k] * m.components[k * col + j]; - } - } - } - - this->CleanUp(); - - return *this; -} -*/ - -Matrix& Matrix::operator*=(float c) { - for (int i = 0; i < row; i++) { - for (int j = 0; j < col; j++) { - components[i*col + j] *= c; - } - } - - return *this; -} - -Matrix& Matrix::operator/=(float c) { - if (fabs(c) < NEARLY_ZERO) error("Division by Zero"); - for (int i = 0; i < row; i++) { - for (int j = 0; j < col; j++) { - components[i*col + j] /= c; - } - } - - return *this; -} - -void Matrix::SetComp(int rowNo, int colNo, float val) { - if (rowNo > row || colNo > col) error("Index Out of Bounds Error"); - components[(rowNo-1)*col + (colNo-1)] = val; -} - -void Matrix::SetComps(float* pComps) { - memcpy(components, pComps, sizeof(float) * row * col); -} - -float Matrix::Determinant() const{ - if (row != col) error("failed to calculate det. : matrix is not square"); - int decSign = 0; - float retVal = 1.0f; - - // 行列のLU分解 - Matrix LU(this->LU_Decompose(&decSign)); - - for (int i = 0; i < LU.row; i++) { - retVal *= LU.components[i * LU.col + i]; - } - - return retVal*decSign; -} - -float Matrix::det() const { - if (row != col) error("failed to calculate det : matrix is not square"); - - Matrix temp(*this); - int decSign = 1; - - for (int j = 0; j < col - 1; j++) { - - // 列内のみで最大の要素を探す - int maxNo = j; - for (int k = j; k < row; k++) { - if (temp.components[maxNo * col + j] < temp.components[k * col + j]) maxNo = k; - } - if (maxNo != j) { - temp.SwapRow(j + 1, maxNo + 1); - decSign *= -1; - } - // 列内の最大要素が小さ過ぎる場合、行内の最大要素も探す - if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) { - maxNo = j; - for (int k = j; k < col; k++) { - if (temp.components[j * col + maxNo] < temp.components[j * col + k])maxNo = k; - } - if (maxNo != j) { - temp.SwapCol(j + 1, maxNo + 1); - decSign *= -1; - } - - // 列内、行内の最大要素を選んでも小さすぎる場合はエラー - if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) { - if (row != col) error("failed to calculate det : Division by Zero"); - } - } - - float c1 = 1.0f / temp.components[j * col + j]; - - for (int i = j + 1; i < row; i++) { - float c2 = temp.components[i * col + j] * c1; - for (int k = j; k < col; k++) { - temp.components[i * col + k] = temp.components[i * col + k] - c2 * temp.components[j * col + k]; - } - } - - } - - if (fabs(temp.components[(row - 1) * col + (col - 1)]) < NEARLY_ZERO) return 0.0f; - - float retVal = 1.0f; - for (int i = 0; i < row; i++) { - retVal *= temp.components[i * col + i]; - } - - return retVal * decSign; -} - -Matrix Matrix::LU_Decompose(int* sign, Matrix* p) const{ - if (row != col) error("failed to LU decomposition: matrix is not square"); - if (sign != 0) *sign = 1; - if (p != 0) { - if (p->row != row || p->row != p->col) error("failed to LU decomposition: permitation matrix is incorrect"); - // 置換行列は最初に単位行列にしておく - memset(p->components, 0, sizeof(float) * row * col); - for (int i = 0; i < row; i++) { - p->components[i * col + i] = 1.0f; - } - } - Matrix retVal(*this); - - for (int d = 0; d < row - 1; d++) { // 1行1列ずつ分解を行う - // d列目の最大の要素を探索し、見つけた要素の行とd行目を交換する - int maxNo = d; - for (int i = d; i < row; i++) { - if (retVal.components[i * col + d] > retVal.components[maxNo * col + d]) maxNo = i; - } - if (maxNo != d) { - retVal.SwapRow(d + 1, maxNo + 1); - if (sign != 0) *sign *= -1; - if (p != 0) { - p->SwapRow(d + 1, maxNo + 1); - } - } - float c = retVal.components[d * col + d]; - if (fabs(c) < NEARLY_ZERO) error("failed to LU decomposition: Division by Zero"); - - // d行d列目以降の行列について計算 - for (int i = d+1; i < row; i++) { - retVal.components[i * col + d] /= c; - for (int j = d+1; j < col; j++) { - retVal.components[i * col + j] -= retVal.components[d * col + j] * retVal.components[i * col + d]; - } - } - } - - retVal.CleanUp(); - - return retVal; -} - -float Matrix::Inverse(Matrix& invm) const{ - if (row != col) error("failed to get Inv. : matrix is not square"); - - Matrix P(*this); - Matrix LU(LU_Decompose(0, &P)); - - // 分解した行列の対角成分の積から行列式を求める - // det = 0 ならfalse - float det = 1.0f; - for (int i = 0; i < row; i++) { - det *= LU.components[i * col + i]; - } - if (fabs(det) < NEARLY_ZERO) { - return fabs(det); - } - - // U、Lそれぞれの逆行列を計算する - Matrix U_inv = Matrix(row, col); - Matrix L_inv = Matrix(row, col); - - for (int j = 0; j < col; j++) { - for (int i = 0; i <= j; i++) { - int i_U = j - i; // U行列の逆行列は対角成分から上へ向かって - // 左から順番に値を計算する - - int j_L = col - 1 - j; // L行列の逆行列は右から順番に - int i_L = j_L + i; // 対角成分から下へ向かって計算する - - if (i_U != j) { // 非対角成分 - float temp_U = 0.0f; - float temp_L = 0.0f; - - for (int k = 0; k < i; k++) { - - temp_U -= U_inv.components[(j - k) * col + j] * LU.components[i_U * col + (j - k)]; - - if (k == 0) { - temp_L -= LU.components[i_L * col + j_L]; - } else { - temp_L -= L_inv.components[(j_L + k) * col + j_L] * LU.components[i_L * col + j_L + k]; - } - - } - - U_inv.components[i_U * col + j] = temp_U / LU.components[i_U * col + i_U]; - L_inv.components[i_L * col + j_L] = temp_L; - - } else { // 対角成分 - if (fabs(LU.components[i_U * col + i_U]) >= NEARLY_ZERO) { - U_inv.components[i_U * col + i_U] = 1.0f / LU.components[i_U * col + i_U]; - } - } - } - } - - invm = U_inv * L_inv * P; - - return -1.0f; -} - -Matrix Matrix::Transpose() const{ - //if (row != col) error("failed to get Trans. : matrix is not square"); - Matrix retVal(col, row); - - for (int i = 0; i < row; i++) { - for (int j = 0; j < col; j++) { - retVal.components[j * row + i] = this->components[i * col + j]; - } - } - - return retVal; -} - -Matrix operator+(const Matrix& lhm, const Matrix& rhm) { - Matrix temp = Matrix(lhm); - temp += rhm; - return temp; -} - -Matrix operator-(const Matrix& lhm, const Matrix& rhm) { - Matrix temp = Matrix(lhm); - temp -= rhm; - return temp; -} - -Matrix operator*(const Matrix& lhm, const Matrix& rhm) { - if(lhm.GetCol() != rhm.GetRow()) error("Matrix product Error: Irregular Dimention."); - int row = lhm.GetRow(); - int col = rhm.GetCol(); - int sum = lhm.GetCol(); - Matrix temp(row, col); - - for (int i = 1; i <= row; i++) { - for (int j = 1; j <= col; j++) { - float temp_c = 0.0f; - for (int k = 1; k <= sum; k++) { - temp_c += lhm.GetComp(i, k) * rhm.GetComp(k, j); - } - temp.SetComp(i, j, temp_c); - } - } - - return temp; -} - -void Matrix::CleanUp() { - int num = row*col; - float maxComp = 0.0f; - for (int i = 0; i < num; i++) { - if (maxComp < fabs(components[i])) maxComp = fabs(components[i]); - } - if (maxComp > NEARLY_ZERO) { - for (int i = 0; i < num; i++) { - if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f; - } - } -} - -void Matrix::SwapRow(int rowNo1, int rowNo2) { - if (rowNo1 > row || rowNo2 > row) error("Index Out of Bounds Error !!"); - float* temp = new float[col]; - - memcpy(temp, components + (rowNo1 - 1) * col, sizeof(float) * col); - memcpy(components + (rowNo1 - 1) * col, components + (rowNo2 - 1) * col, sizeof(float) * col); - memcpy(components + (rowNo2 - 1) * col, temp, sizeof(float) * col); - - delete[] temp; -} - -void Matrix::SwapCol(int colNo1, int colNo2) { - if (colNo1 > col || colNo2 > col) error("Index Out of Bounds Error !!"); - float temp = 0.0f; - - for (int i = 0; i < row; i++) { - temp = components[i * col + colNo1]; - components[i * col + colNo1] = components[i * col + colNo2]; - components[i * col + colNo2] = temp; - } -} \ No newline at end of file