mirror of https://github.com/opencv/opencv.git
Open Source Computer Vision Library
https://opencv.org/
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
295 lines
10 KiB
295 lines
10 KiB
/* |
|
* The copyright in this software is being made available under the 2-clauses |
|
* BSD License, included below. This software may be subject to other third |
|
* party and contributor rights, including patent rights, and no such rights |
|
* are granted under this license. |
|
* |
|
* Copyright (c) 2008, Jerome Fimes, Communications & Systemes <jerome.fimes@c-s.fr> |
|
* All rights reserved. |
|
* |
|
* Redistribution and use in source and binary forms, with or without |
|
* modification, are permitted provided that the following conditions |
|
* are met: |
|
* 1. Redistributions of source code must retain the above copyright |
|
* notice, this list of conditions and the following disclaimer. |
|
* 2. Redistributions in binary form must reproduce the above copyright |
|
* notice, this list of conditions and the following disclaimer in the |
|
* documentation and/or other materials provided with the distribution. |
|
* |
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS `AS IS' |
|
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
|
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
|
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
|
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
|
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
|
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
|
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
|
* POSSIBILITY OF SUCH DAMAGE. |
|
*/ |
|
|
|
#include "opj_includes.h" |
|
|
|
/** |
|
* LUP decomposition |
|
*/ |
|
static OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix, |
|
OPJ_UINT32 * permutations, |
|
OPJ_FLOAT32 * p_swap_area, |
|
OPJ_UINT32 nb_compo); |
|
/** |
|
* LUP solving |
|
*/ |
|
static void opj_lupSolve(OPJ_FLOAT32 * pResult, |
|
OPJ_FLOAT32* pMatrix, |
|
OPJ_FLOAT32* pVector, |
|
OPJ_UINT32* pPermutations, |
|
OPJ_UINT32 nb_compo, |
|
OPJ_FLOAT32 * p_intermediate_data); |
|
|
|
/** |
|
*LUP inversion (call with the result of lupDecompose) |
|
*/ |
|
static void opj_lupInvert(OPJ_FLOAT32 * pSrcMatrix, |
|
OPJ_FLOAT32 * pDestMatrix, |
|
OPJ_UINT32 nb_compo, |
|
OPJ_UINT32 * pPermutations, |
|
OPJ_FLOAT32 * p_src_temp, |
|
OPJ_FLOAT32 * p_dest_temp, |
|
OPJ_FLOAT32 * p_swap_area); |
|
|
|
/* |
|
========================================================== |
|
Matric inversion interface |
|
========================================================== |
|
*/ |
|
/** |
|
* Matrix inversion. |
|
*/ |
|
OPJ_BOOL opj_matrix_inversion_f(OPJ_FLOAT32 * pSrcMatrix, |
|
OPJ_FLOAT32 * pDestMatrix, |
|
OPJ_UINT32 nb_compo) |
|
{ |
|
OPJ_BYTE * l_data = 00; |
|
OPJ_UINT32 l_permutation_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_UINT32); |
|
OPJ_UINT32 l_swap_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); |
|
OPJ_UINT32 l_total_size = l_permutation_size + 3 * l_swap_size; |
|
OPJ_UINT32 * lPermutations = 00; |
|
OPJ_FLOAT32 * l_double_data = 00; |
|
|
|
l_data = (OPJ_BYTE *) opj_malloc(l_total_size); |
|
if (l_data == 0) { |
|
return OPJ_FALSE; |
|
} |
|
lPermutations = (OPJ_UINT32 *) l_data; |
|
l_double_data = (OPJ_FLOAT32 *)(l_data + l_permutation_size); |
|
memset(lPermutations, 0, l_permutation_size); |
|
|
|
if (! opj_lupDecompose(pSrcMatrix, lPermutations, l_double_data, nb_compo)) { |
|
opj_free(l_data); |
|
return OPJ_FALSE; |
|
} |
|
|
|
opj_lupInvert(pSrcMatrix, pDestMatrix, nb_compo, lPermutations, l_double_data, |
|
l_double_data + nb_compo, l_double_data + 2 * nb_compo); |
|
opj_free(l_data); |
|
|
|
return OPJ_TRUE; |
|
} |
|
|
|
|
|
/* |
|
========================================================== |
|
Local functions |
|
========================================================== |
|
*/ |
|
static OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix, |
|
OPJ_UINT32 * permutations, |
|
OPJ_FLOAT32 * p_swap_area, |
|
OPJ_UINT32 nb_compo) |
|
{ |
|
OPJ_UINT32 * tmpPermutations = permutations; |
|
OPJ_UINT32 * dstPermutations; |
|
OPJ_UINT32 k2 = 0, t; |
|
OPJ_FLOAT32 temp; |
|
OPJ_UINT32 i, j, k; |
|
OPJ_FLOAT32 p; |
|
OPJ_UINT32 lLastColum = nb_compo - 1; |
|
OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); |
|
OPJ_FLOAT32 * lTmpMatrix = matrix; |
|
OPJ_FLOAT32 * lColumnMatrix, * lDestMatrix; |
|
OPJ_UINT32 offset = 1; |
|
OPJ_UINT32 lStride = nb_compo - 1; |
|
|
|
/*initialize permutations */ |
|
for (i = 0; i < nb_compo; ++i) { |
|
*tmpPermutations++ = i; |
|
} |
|
/* now make a pivot with column switch */ |
|
tmpPermutations = permutations; |
|
for (k = 0; k < lLastColum; ++k) { |
|
p = 0.0; |
|
|
|
/* take the middle element */ |
|
lColumnMatrix = lTmpMatrix + k; |
|
|
|
/* make permutation with the biggest value in the column */ |
|
for (i = k; i < nb_compo; ++i) { |
|
temp = ((*lColumnMatrix > 0) ? *lColumnMatrix : -(*lColumnMatrix)); |
|
if (temp > p) { |
|
p = temp; |
|
k2 = i; |
|
} |
|
/* next line */ |
|
lColumnMatrix += nb_compo; |
|
} |
|
|
|
/* a whole rest of 0 -> non singular */ |
|
if (p == 0.0) { |
|
return OPJ_FALSE; |
|
} |
|
|
|
/* should we permute ? */ |
|
if (k2 != k) { |
|
/*exchange of line */ |
|
/* k2 > k */ |
|
dstPermutations = tmpPermutations + k2 - k; |
|
/* swap indices */ |
|
t = *tmpPermutations; |
|
*tmpPermutations = *dstPermutations; |
|
*dstPermutations = t; |
|
|
|
/* and swap entire line. */ |
|
lColumnMatrix = lTmpMatrix + (k2 - k) * nb_compo; |
|
memcpy(p_swap_area, lColumnMatrix, lSwapSize); |
|
memcpy(lColumnMatrix, lTmpMatrix, lSwapSize); |
|
memcpy(lTmpMatrix, p_swap_area, lSwapSize); |
|
} |
|
|
|
/* now update data in the rest of the line and line after */ |
|
lDestMatrix = lTmpMatrix + k; |
|
lColumnMatrix = lDestMatrix + nb_compo; |
|
/* take the middle element */ |
|
temp = *(lDestMatrix++); |
|
|
|
/* now compute up data (i.e. coeff up of the diagonal). */ |
|
for (i = offset; i < nb_compo; ++i) { |
|
/*lColumnMatrix; */ |
|
/* divide the lower column elements by the diagonal value */ |
|
|
|
/* matrix[i][k] /= matrix[k][k]; */ |
|
/* p = matrix[i][k] */ |
|
p = *lColumnMatrix / temp; |
|
*(lColumnMatrix++) = p; |
|
|
|
for (j = /* k + 1 */ offset; j < nb_compo; ++j) { |
|
/* matrix[i][j] -= matrix[i][k] * matrix[k][j]; */ |
|
*(lColumnMatrix++) -= p * (*(lDestMatrix++)); |
|
} |
|
/* come back to the k+1th element */ |
|
lDestMatrix -= lStride; |
|
/* go to kth element of the next line */ |
|
lColumnMatrix += k; |
|
} |
|
|
|
/* offset is now k+2 */ |
|
++offset; |
|
/* 1 element less for stride */ |
|
--lStride; |
|
/* next line */ |
|
lTmpMatrix += nb_compo; |
|
/* next permutation element */ |
|
++tmpPermutations; |
|
} |
|
return OPJ_TRUE; |
|
} |
|
|
|
static void opj_lupSolve(OPJ_FLOAT32 * pResult, |
|
OPJ_FLOAT32 * pMatrix, |
|
OPJ_FLOAT32 * pVector, |
|
OPJ_UINT32* pPermutations, |
|
OPJ_UINT32 nb_compo, OPJ_FLOAT32 * p_intermediate_data) |
|
{ |
|
OPJ_INT32 k; |
|
OPJ_UINT32 i, j; |
|
OPJ_FLOAT32 sum; |
|
OPJ_FLOAT32 u; |
|
OPJ_UINT32 lStride = nb_compo + 1; |
|
OPJ_FLOAT32 * lCurrentPtr; |
|
OPJ_FLOAT32 * lIntermediatePtr; |
|
OPJ_FLOAT32 * lDestPtr; |
|
OPJ_FLOAT32 * lTmpMatrix; |
|
OPJ_FLOAT32 * lLineMatrix = pMatrix; |
|
OPJ_FLOAT32 * lBeginPtr = pResult + nb_compo - 1; |
|
OPJ_FLOAT32 * lGeneratedData; |
|
OPJ_UINT32 * lCurrentPermutationPtr = pPermutations; |
|
|
|
|
|
lIntermediatePtr = p_intermediate_data; |
|
lGeneratedData = p_intermediate_data + nb_compo - 1; |
|
|
|
for (i = 0; i < nb_compo; ++i) { |
|
sum = 0.0; |
|
lCurrentPtr = p_intermediate_data; |
|
lTmpMatrix = lLineMatrix; |
|
for (j = 1; j <= i; ++j) { |
|
/* sum += matrix[i][j-1] * y[j-1]; */ |
|
sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++)); |
|
} |
|
/*y[i] = pVector[pPermutations[i]] - sum; */ |
|
*(lIntermediatePtr++) = pVector[*(lCurrentPermutationPtr++)] - sum; |
|
lLineMatrix += nb_compo; |
|
} |
|
|
|
/* we take the last point of the matrix */ |
|
lLineMatrix = pMatrix + nb_compo * nb_compo - 1; |
|
|
|
/* and we take after the last point of the destination vector */ |
|
lDestPtr = pResult + nb_compo; |
|
|
|
|
|
assert(nb_compo != 0); |
|
for (k = (OPJ_INT32)nb_compo - 1; k != -1 ; --k) { |
|
sum = 0.0; |
|
lTmpMatrix = lLineMatrix; |
|
u = *(lTmpMatrix++); |
|
lCurrentPtr = lDestPtr--; |
|
for (j = (OPJ_UINT32)(k + 1); j < nb_compo; ++j) { |
|
/* sum += matrix[k][j] * x[j] */ |
|
sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++)); |
|
} |
|
/*x[k] = (y[k] - sum) / u; */ |
|
*(lBeginPtr--) = (*(lGeneratedData--) - sum) / u; |
|
lLineMatrix -= lStride; |
|
} |
|
} |
|
|
|
|
|
static void opj_lupInvert(OPJ_FLOAT32 * pSrcMatrix, |
|
OPJ_FLOAT32 * pDestMatrix, |
|
OPJ_UINT32 nb_compo, |
|
OPJ_UINT32 * pPermutations, |
|
OPJ_FLOAT32 * p_src_temp, |
|
OPJ_FLOAT32 * p_dest_temp, |
|
OPJ_FLOAT32 * p_swap_area) |
|
{ |
|
OPJ_UINT32 j, i; |
|
OPJ_FLOAT32 * lCurrentPtr; |
|
OPJ_FLOAT32 * lLineMatrix = pDestMatrix; |
|
OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32); |
|
|
|
for (j = 0; j < nb_compo; ++j) { |
|
lCurrentPtr = lLineMatrix++; |
|
memset(p_src_temp, 0, lSwapSize); |
|
p_src_temp[j] = 1.0; |
|
opj_lupSolve(p_dest_temp, pSrcMatrix, p_src_temp, pPermutations, nb_compo, |
|
p_swap_area); |
|
|
|
for (i = 0; i < nb_compo; ++i) { |
|
*(lCurrentPtr) = p_dest_temp[i]; |
|
lCurrentPtr += nb_compo; |
|
} |
|
} |
|
} |
|
|
|
|