494 lines
16 KiB
C++
494 lines
16 KiB
C++
/*
|
|
* Copyright (c) 2014 Amlogic, Inc. All rights reserved.
|
|
*
|
|
* This source code is subject to the terms and conditions defined in the
|
|
* file 'LICENSE' which is part of this source code package.
|
|
*
|
|
* Description: header file
|
|
*/
|
|
#ifndef _GAMMAOPEATION_H
|
|
#define _GAMMAOPEATION_H
|
|
|
|
#include <cstdio>
|
|
#include <cassert>
|
|
#include <vector>
|
|
#include <algorithm>
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
|
|
//#include "PQType.h"
|
|
//#include "CPQLog.h"
|
|
|
|
// unnamed namespace only because the implementation is in this
|
|
// header file and we don't want to export symbols to the obj files
|
|
namespace
|
|
{
|
|
|
|
namespace tk
|
|
{
|
|
|
|
// band matrix solver
|
|
class band_matrix
|
|
{
|
|
private:
|
|
std::vector< std::vector<double> > m_upper; // upper band
|
|
std::vector< std::vector<double> > m_lower; // lower band
|
|
public:
|
|
band_matrix() {}; // constructor
|
|
band_matrix(int dim, int n_u, int n_l); // constructor
|
|
~band_matrix() {}; // destructor
|
|
void resize(int dim, int n_u, int n_l); // init with dim,n_u,n_l
|
|
int dim() const; // matrix dimension
|
|
int num_upper() const
|
|
{
|
|
return m_upper.size()-1;
|
|
}
|
|
int num_lower() const
|
|
{
|
|
return m_lower.size()-1;
|
|
}
|
|
// access operator
|
|
double & operator () (int i, int j); // write
|
|
double operator () (int i, int j) const; // read
|
|
// we can store an additional diogonal (in m_lower)
|
|
double& saved_diag(int i);
|
|
double saved_diag(int i) const;
|
|
void lu_decompose();
|
|
std::vector<double> r_solve(const std::vector<double>& b) const;
|
|
std::vector<double> l_solve(const std::vector<double>& b) const;
|
|
std::vector<double> lu_solve(const std::vector<double>& b,
|
|
bool is_lu_decomposed=false);
|
|
|
|
};
|
|
|
|
// band_matrix implementation
|
|
// -------------------------
|
|
|
|
band_matrix::band_matrix(int dim, int n_u, int n_l)
|
|
{
|
|
resize(dim, n_u, n_l);
|
|
}
|
|
void band_matrix::resize(int dim, int n_u, int n_l)
|
|
{
|
|
assert(dim > 0);
|
|
assert(n_u >= 0);
|
|
assert(n_l >= 0);
|
|
m_upper.resize(n_u + 1);
|
|
m_lower.resize(n_l + 1);
|
|
for (size_t i = 0; i < m_upper.size(); i++) {
|
|
m_upper[i].resize(dim);
|
|
}
|
|
for (size_t i = 0; i < m_lower.size(); i++) {
|
|
m_lower[i].resize(dim);
|
|
}
|
|
}
|
|
|
|
int band_matrix::dim() const
|
|
{
|
|
if (m_upper.size() > 0) {
|
|
return m_upper[0].size();
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// defines the new operator (), so that we can access the elements
|
|
// by A(i,j), index going from i=0,...,dim()-1
|
|
double & band_matrix::operator () (int i, int j)
|
|
{
|
|
int k = j - i; // what band is the entry
|
|
assert((i >= 0) && (i < dim()) && (j >= 0) && (j < dim()));
|
|
assert((-num_lower() <= k) && (k <= num_upper()));
|
|
// k=0 -> diogonal, k<0 lower left part, k>0 upper right part
|
|
if (k >= 0) return m_upper[k][i];
|
|
else return m_lower[-k][i];
|
|
}
|
|
double band_matrix::operator () (int i, int j) const
|
|
{
|
|
int k = j - i; // what band is the entry
|
|
assert((i >= 0) && (i < dim()) && (j >= 0) && (j < dim()));
|
|
assert((-num_lower() <= k) && (k <= num_upper()));
|
|
// k=0 -> diogonal, k<0 lower left part, k>0 upper right part
|
|
if (k >= 0) return m_upper[k][i];
|
|
else return m_lower[-k][i];
|
|
}
|
|
// second diag (used in LU decomposition), saved in m_lower
|
|
double band_matrix::saved_diag(int i) const
|
|
{
|
|
assert( (i >= 0) && (i < dim()) );
|
|
return m_lower[0][i];
|
|
}
|
|
double & band_matrix::saved_diag(int i)
|
|
{
|
|
assert( (i >= 0) && (i < dim()) );
|
|
return m_lower[0][i];
|
|
}
|
|
|
|
// LR-Decomposition of a band matrix
|
|
void band_matrix::lu_decompose()
|
|
{
|
|
int i_max,j_max;
|
|
int j_min;
|
|
double x;
|
|
|
|
// preconditioning
|
|
// normalize column i so that a_ii=1
|
|
for (int i = 0; i < this->dim(); i++) {
|
|
assert(this->operator()(i,i) != 0.0);
|
|
this->saved_diag(i) = 1.0 / this->operator()(i,i);
|
|
j_min = std::max(0,i-this->num_lower());
|
|
j_max = std::min(this->dim() - 1,i + this->num_upper());
|
|
for(int j = j_min; j <= j_max; j++) {
|
|
this->operator()(i,j) *= this->saved_diag(i);
|
|
}
|
|
this->operator()(i,i) = 1.0; // prevents rounding errors
|
|
}
|
|
|
|
// Gauss LR-Decomposition
|
|
for (int k = 0; k < this->dim(); k++) {
|
|
i_max = std::min(this->dim()-1,k + this->num_lower()); // num_lower not a mistake!
|
|
for (int i = k + 1; i <= i_max; i++) {
|
|
assert(this->operator()(k,k) != 0.0);
|
|
x = -this->operator()(i,k) / this->operator()(k,k);
|
|
this->operator()(i,k) = -x; // assembly part of L
|
|
j_max=std::min(this->dim() - 1,k + this->num_upper());
|
|
for (int j = k + 1; j <= j_max; j++) {
|
|
// assembly part of R
|
|
this->operator()(i,j) = this->operator()(i,j) + x * this->operator()(k,j);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
// solves Ly=b
|
|
std::vector<double> band_matrix::l_solve(const std::vector<double>& b) const
|
|
{
|
|
assert(this->dim() == (int)b.size());
|
|
std::vector<double> x(this->dim());
|
|
int j_start;
|
|
double sum;
|
|
for (int i = 0; i < this->dim(); i++) {
|
|
sum = 0;
|
|
j_start = std::max(0,i - this->num_lower());
|
|
for (int j = j_start; j < i; j++) sum += this->operator()(i,j) * x[j];
|
|
x[i] = (b[i] * this->saved_diag(i)) - sum;
|
|
}
|
|
return x;
|
|
}
|
|
// solves Rx=y
|
|
std::vector<double> band_matrix::r_solve(const std::vector<double>& b) const
|
|
{
|
|
assert( this->dim() == (int)b.size() );
|
|
std::vector<double> x(this->dim());
|
|
int j_stop;
|
|
double sum;
|
|
for (int i = this->dim() - 1; i >= 0; i--) {
|
|
sum = 0;
|
|
j_stop = std::min(this->dim() - 1,i + this->num_upper());
|
|
for (int j = i + 1; j <= j_stop; j++) sum += this->operator()(i,j)*x[j];
|
|
x[i] = ( b[i] - sum ) / this->operator()(i,i);
|
|
}
|
|
return x;
|
|
}
|
|
|
|
std::vector<double> band_matrix::lu_solve(const std::vector<double>& b,
|
|
bool is_lu_decomposed)
|
|
{
|
|
assert( this->dim() == (int)b.size() );
|
|
std::vector<double> x,y;
|
|
if (is_lu_decomposed == false) {
|
|
this->lu_decompose();
|
|
}
|
|
y = this->l_solve(b);
|
|
x = this->r_solve(y);
|
|
return x;
|
|
}
|
|
|
|
// spline interpolation
|
|
class spline
|
|
{
|
|
public:
|
|
enum bd_type {
|
|
first_deriv = 1,
|
|
second_deriv = 2
|
|
};
|
|
|
|
private:
|
|
std::vector<double> m_x,m_y; // x,y coordinates of points
|
|
// interpolation parameters
|
|
// f(x) = a*(x-x_i)^3 + b*(x-x_i)^2 + c*(x-x_i) + y_i
|
|
std::vector<double> m_a,m_b,m_c; // spline coefficients
|
|
double m_b0 = 0.0, m_c0 = 0.0; // for left extrapol
|
|
bd_type m_left, m_right;
|
|
double m_left_value, m_right_value;
|
|
bool m_force_linear_extrapolation;
|
|
|
|
public:
|
|
// set default boundary condition to be zero curvature at both ends
|
|
spline(): m_left(second_deriv), m_right(second_deriv),
|
|
m_left_value(0.0), m_right_value(0.0),
|
|
m_force_linear_extrapolation(false)
|
|
{
|
|
;
|
|
}
|
|
|
|
// optional, but if called it has to come be before set_points()
|
|
void set_boundary(bd_type left, double left_value, bd_type right, double right_value, bool force_linear_extrapolation = false);
|
|
void set_points(const std::vector<double>& x, const std::vector<double>& y, bool cubic_spline = true);
|
|
double operator() (double x) const;
|
|
double deriv(int order, double x) const;
|
|
};
|
|
|
|
|
|
// ---------------------------------------------------------------------
|
|
// implementation part, which could be separated into a cpp file
|
|
// ---------------------------------------------------------------------
|
|
|
|
|
|
// spline implementation
|
|
// -----------------------
|
|
__attribute__((unused)) void spline::set_boundary(spline::bd_type left, double left_value,
|
|
spline::bd_type right, double right_value,
|
|
bool force_linear_extrapolation)
|
|
{
|
|
assert(m_x.size() == 0); // set_points() must not have happened yet
|
|
m_left = left;
|
|
m_right = right;
|
|
m_left_value = left_value;
|
|
m_right_value = right_value;
|
|
m_force_linear_extrapolation = force_linear_extrapolation;
|
|
}
|
|
|
|
__attribute__((unused)) void spline::set_points(const std::vector<double>& x,
|
|
const std::vector<double>& y, bool cubic_spline)
|
|
{
|
|
assert(x.size() == y.size());
|
|
assert(x.size() > 2);
|
|
m_x = x;
|
|
m_y = y;
|
|
int n = x.size();
|
|
// TODO: maybe sort x and y, rather than returning an error
|
|
for (int i = 0; i < n - 1; i++) {
|
|
assert(m_x[i] < m_x[i + 1]);
|
|
}
|
|
|
|
if (cubic_spline == true) { // cubic spline interpolation
|
|
// setting up the matrix and right hand side of the equation system
|
|
// for the parameters b[]
|
|
band_matrix A(n, 1, 1);
|
|
std::vector<double> rhs(n);
|
|
for (int i = 1; i < n - 1; i++) {
|
|
A(i, i - 1) = 1.0 / 3.0 * (x[i] - x[i - 1]);
|
|
A(i, i) = 2.0 / 3.0 * (x[i + 1] - x[i - 1]);
|
|
A(i, i + 1) = 1.0 / 3.0 * (x[i + 1] - x[i]);
|
|
rhs[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) - (y[i] - y[i-1]) / (x[i]-x[i - 1]);
|
|
}
|
|
// boundary conditions
|
|
if (m_left == spline::second_deriv) {
|
|
// 2*b[0] = f''
|
|
A(0, 0) = 2.0;
|
|
A(0, 1) = 0.0;
|
|
rhs[0] = m_left_value;
|
|
} else if (m_left == spline::first_deriv) {
|
|
// c[0] = f', needs to be re-expressed in terms of b:
|
|
// (2b[0]+b[1])(x[1]-x[0]) = 3 ((y[1]-y[0])/(x[1]-x[0]) - f')
|
|
A(0, 0) = 2.0 * (x[1] - x[0]);
|
|
A(0, 1) = 1.0 * (x[1] - x[0]);
|
|
rhs[0] = 3.0 * ((y[1] - y[0]) / (x[1] - x[0]) - m_left_value);
|
|
} else {
|
|
assert(false);
|
|
}
|
|
if (m_right == spline::second_deriv) {
|
|
// 2*b[n-1] = f''
|
|
A(n - 1,n - 1) = 2.0;
|
|
A(n - 1,n - 2) = 0.0;
|
|
rhs[n - 1] = m_right_value;
|
|
} else if (m_right == spline::first_deriv) {
|
|
// c[n-1] = f', needs to be re-expressed in terms of b:
|
|
// (b[n-2]+2b[n-1])(x[n-1]-x[n-2])
|
|
// = 3 (f' - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]))
|
|
A(n - 1,n - 1) = 2.0 * (x[n - 1] - x[n - 2]);
|
|
A(n - 1,n - 2) = 1.0 * (x[n - 1] - x[n - 2]);
|
|
rhs[n - 1] = 3.0 * (m_right_value - (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2]));
|
|
} else {
|
|
assert(false);
|
|
}
|
|
|
|
// solve the equation system to obtain the parameters b[]
|
|
m_b = A.lu_solve(rhs);
|
|
|
|
// calculate parameters a[] and c[] based on b[]
|
|
m_a.resize(n);
|
|
m_c.resize(n);
|
|
for (int i = 0; i < n - 1; i++) {
|
|
m_a[i] = 1.0 / 3.0 * (m_b[i + 1] - m_b[i]) / (x[i + 1] - x[i]);
|
|
m_c[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i])
|
|
- 1.0 / 3.0 * (2.0 * m_b[i] + m_b[i + 1]) * (x[i + 1] - x[i]);
|
|
}
|
|
} else { // linear interpolation
|
|
m_a.resize(n);
|
|
m_b.resize(n);
|
|
m_c.resize(n);
|
|
for (int i = 0; i < n - 1; i++) {
|
|
m_a[i] = 0.0;
|
|
m_b[i] = 0.0;
|
|
m_c[i] = (m_y[i + 1] - m_y[i]) / (m_x[i + 1] - m_x[i]);
|
|
}
|
|
}
|
|
|
|
// for left extrapolation coefficients
|
|
m_b0 = (m_force_linear_extrapolation == false) ? m_b[0] : 0.0;
|
|
m_c0 = m_c[0];
|
|
|
|
// for the right extrapolation coefficients
|
|
// f_{n-1}(x) = b*(x-x_{n-1})^2 + c*(x-x_{n-1}) + y_{n-1}
|
|
double h = x[n - 1] - x[n - 2];
|
|
// m_b[n-1] is determined by the boundary condition
|
|
m_a[n - 1] = 0.0;
|
|
m_c[n - 1] = 3.0 * m_a[n - 2] * h * h + 2.0 * m_b[n - 2] * h + m_c[n - 2]; // = f'_{n-2}(x_{n-1})
|
|
if (m_force_linear_extrapolation == true)
|
|
m_b[n - 1] = 0.0;
|
|
}
|
|
|
|
__attribute__((unused)) double spline::operator() (double x) const
|
|
{
|
|
size_t n = m_x.size();
|
|
// find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
|
|
std::vector<double>::const_iterator it;
|
|
it = std::lower_bound(m_x.begin(),m_x.end(),x);
|
|
int idx = std::max( int(it-m_x.begin())-1, 0);
|
|
|
|
double h = x - m_x[idx];
|
|
double interpol;
|
|
if (x < m_x[0]) {
|
|
// extrapolation to the left
|
|
interpol = (m_b0 * h + m_c0) * h + m_y[0];
|
|
} else if (x > m_x[n - 1]) {
|
|
// extrapolation to the right
|
|
interpol = (m_b[n - 1] * h + m_c[n - 1]) * h + m_y[n-1];
|
|
} else {
|
|
// interpolation
|
|
interpol = ((m_a[idx] * h + m_b[idx]) * h + m_c[idx]) * h + m_y[idx];
|
|
}
|
|
return interpol;
|
|
}
|
|
|
|
|
|
__attribute__((unused)) double spline::deriv(int order, double x) const
|
|
{
|
|
assert(order>0);
|
|
|
|
size_t n = m_x.size();
|
|
// find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
|
|
std::vector<double>::const_iterator it;
|
|
it=std::lower_bound(m_x.begin(),m_x.end(),x);
|
|
int idx=std::max( int(it-m_x.begin())-1, 0);
|
|
|
|
double h = x - m_x[idx];
|
|
double interpol;
|
|
if (x < m_x[0]) {
|
|
// extrapolation to the left
|
|
switch(order) {
|
|
case 1:
|
|
interpol = 2.0 * m_b0 * h + m_c0;
|
|
break;
|
|
case 2:
|
|
interpol = 2.0 * m_b0 * h;
|
|
break;
|
|
default:
|
|
interpol = 0.0;
|
|
break;
|
|
}
|
|
} else if (x > m_x[n - 1]) {
|
|
// extrapolation to the right
|
|
switch(order) {
|
|
case 1:
|
|
interpol = 2.0 * m_b[n - 1] * h + m_c[n - 1];
|
|
break;
|
|
case 2:
|
|
interpol = 2.0 * m_b[n - 1];
|
|
break;
|
|
default:
|
|
interpol = 0.0;
|
|
break;
|
|
}
|
|
} else {
|
|
// interpolation
|
|
switch(order) {
|
|
case 1:
|
|
interpol = (3.0 * m_a[idx] * h + 2.0 * m_b[idx]) * h + m_c[idx];
|
|
break;
|
|
case 2:
|
|
interpol = 6.0 * m_a[idx] * h + 2.0 * m_b[idx];
|
|
break;
|
|
case 3:
|
|
interpol = 6.0 * m_a[idx];
|
|
break;
|
|
default:
|
|
interpol = 0.0;
|
|
break;
|
|
}
|
|
}
|
|
return interpol;
|
|
}
|
|
|
|
|
|
} // namespace tk
|
|
|
|
|
|
} // namespace
|
|
|
|
#ifndef NUMBER_POINTS
|
|
#define NUMBER_POINTS 40
|
|
#endif
|
|
|
|
typedef struct interpolation_info_s {
|
|
float a[NUMBER_POINTS];
|
|
float b[NUMBER_POINTS];
|
|
float c[NUMBER_POINTS];
|
|
float d[NUMBER_POINTS];
|
|
float *x;
|
|
float *y;
|
|
int num_points;
|
|
}interpolation_info_t;
|
|
|
|
class GammaOperation {
|
|
public:
|
|
GammaOperation();
|
|
~GammaOperation();
|
|
static GammaOperation *GetInstance();
|
|
|
|
//gamma power Convert
|
|
int GammaOperation_BaseGammaConvert(unsigned short *gamma_value, double basePower , double targetPower, int NodeNum);
|
|
|
|
//whiteBalanceGamma
|
|
interpolation_info_t* nat_cubic_spline(int num_points, interpolation_info_t* output);
|
|
int evaluate(interpolation_info_t *function, float val, float *result);
|
|
|
|
private:
|
|
static GammaOperation *mInstance;
|
|
//gamma Convert
|
|
int GammaConvert_set_points(const std::vector<double>& y, bool cubic_spline);
|
|
double GammaConvert_Get_points(double x);
|
|
//whiteBalanceGamma
|
|
void build_A_matrix(float *x_delta, int num_points, float A[][NUMBER_POINTS]);
|
|
float* build_h_vector(float *h, float *x_delta, float *a, int num_points);
|
|
float* solve_matrix(float *x, float *h, int num_points, float A[][NUMBER_POINTS]);
|
|
float* build_b_vector(float *b, float *x_delta, float *y_delta, float *c, int num_points);
|
|
float* build_d_vector(float *d, float *x_delta, float *c, int num_points);
|
|
float spline_func(interpolation_info_t *function, float val, int i);
|
|
bool almost_equals(float a, float b);
|
|
|
|
std::vector<double> xData,yData; // x,y coordinates of points
|
|
std::vector< std::vector<double> > m_upper; // upper band
|
|
std::vector< std::vector<double> > m_lower; // lower band
|
|
std::vector<double> m_a,m_b,m_c; // spline coefficients
|
|
};
|
|
|
|
#endif
|
|
|