//f = f./repmat(sum(f,1),[classN,1]);
vnl_matrix<double> MCLR_SM::Normalize_F_Sum(vnl_matrix<double> f)
{
// Matrix for normalization
vnl_matrix<double> norm_matrix(f.rows(),f.cols());
vnl_vector<double> norm_matrix_row;
norm_matrix_row.set_size(f.cols());
//repmat(sum(f,1),[classN,1]);
for(int i=0;i<f.cols();++i)
{
double sum = 0 ;
for(int j=0;j<no_of_classes;++j)
{
sum = sum + f(j,i);
}
norm_matrix_row(i) = sum;
}
for(int i=0;i<no_of_classes;++i)
{
norm_matrix.set_row(i,norm_matrix_row);
}
// f = f./repmat(sum(f,1),[classN,1]);
for(int i=0;i<f.rows();++i)
{
for(int j=0;j<f.cols();++j)
{
f(i,j) = f(i,j)/norm_matrix(i,j);
}
}
return f;
}
void pick_indices(const vnl_matrix<double>&dist,
vnl_matrix<int>&pairs, const double threshold) {
int m = dist.rows();
int n = dist.cols();
vnl_vector<int> row_flag, col_flag;
col_flag.set_size(n); col_flag.fill(0);
row_flag.set_size(n); row_flag.fill(0);
std::vector<int> row_index,col_index;
for (int i = 0; i < m; ++i) {
double min_dist = dist.get_row(i).min_value();
if (min_dist < threshold) {
for (int j = 0; j < n; ++j) {
if (dist(i,j)==min_dist && col_flag[j] == 0){
row_index.push_back(i);
row_flag[i] = 1;
col_index.push_back(j);
col_flag[j] = 1;
}
}
}
}
pairs.set_size(2, row_index.size());
for (int i = 0; i<pairs.cols(); ++i){
pairs(0,i) = row_index[i];
pairs(1,i) = col_index[i];
}
}
bool rgrsn_ldp::local_dynamic_programming(const vnl_matrix<double> & probMap, int nNeighborBin,
vcl_vector<int> & optimalBins)
{
const int N = probMap.rows();
const int nBin = probMap.cols();
// dynamic programming
vnl_matrix<double> accumulatedProbMap = vnl_matrix<double>(N, nBin);
accumulatedProbMap.fill(0.0);
vnl_matrix<int> lookbackTable = vnl_matrix<int>(N, nBin);
lookbackTable.fill(0);
// copy first row
for (int c = 0; c<probMap.cols(); c++) {
accumulatedProbMap[0][c] = probMap[0][c];
lookbackTable[0][c] = c;
}
for (int r = 1; r <N; r++) {
for (int c = 0; c<probMap.cols(); c++) {
// lookup all possible place in the window
double max_val = -1;
int max_index = -1;
for (int w = -nNeighborBin; w <= nNeighborBin; w++) {
if (c + w <0 || c + w >= probMap.cols()) {
continue;
}
double val = probMap[r][c] + accumulatedProbMap[r-1][c+w];
if (val > max_val) {
max_val = val;
max_index = c + w; // most probable path from the [r-1] row, in column c + w
}
}
assert(max_index != -1);
accumulatedProbMap[r][c] = max_val;
lookbackTable[r][c] = max_index;
}
}
// lookback the table
double max_prob = -1.0;
int max_prob_index = -1;
for (int c = 0; c<accumulatedProbMap.cols(); c++) {
if (accumulatedProbMap[N-1][c] > max_prob) {
max_prob = accumulatedProbMap[N-1][c];
max_prob_index = c;
}
}
// back track
optimalBins.push_back(max_prob_index);
for (int r = N-1; r > 0; r--) {
int bin = lookbackTable[r][optimalBins.back()];
optimalBins.push_back(bin);
}
assert(optimalBins.size() == N);
// vcl_reverse(optimalBins.begin(), optimalBins.end());
return true;
}
bool rgrsn_ldp::dynamic_programming(const vnl_matrix<double> & data, double v_min, double v_max,
unsigned int nBin, int nJumpBin, unsigned int windowSize,
vnl_vector<double> & optimalSignal)
{
assert(v_min < v_max);
// raw data to probability map
const int N = data.rows();
vnl_matrix<double> probMap = vnl_matrix<double>(N, nBin);
double interval = (v_max - v_min)/nBin;
for (int r = 0; r<N; r++) {
for (int c = 0; c<data.cols(); c++) {
int num = value_to_bin_number(v_min, interval, data[r][c], nBin);
probMap[r][num] += 1.0;
}
}
probMap /= data.cols(); // normalize
// save prob
{
// vnl_matlab_filewrite awriter("prob.mat");
// awriter.write(probMap, "prob");
// printf("save to prob.mat.\n");
}
vcl_vector<double> optimalValues(N, 0);
vcl_vector<int> numValues(N, 0); // multiple values from local dynamic programming
for (int i = 0; i<=N - windowSize; i++) {
// get a local probMap;
vnl_matrix<double> localProbMap = probMap.extract(windowSize, probMap.cols(), i, 0);
vcl_vector<int> localOptimalBins;
rgrsn_ldp::local_dynamic_programming(localProbMap, nJumpBin, localOptimalBins);
assert(localOptimalBins.size() == windowSize);
for (int j = 0; j < localOptimalBins.size(); j++) {
numValues[j + i] += 1;
optimalValues[j + i] += bin_number_to_value(v_min, interval, localOptimalBins[j]);
}
// test
if (0 && i == 0)
{
printf("test first window output\n");
for (int j = 0; j<optimalValues.size() && j<windowSize; j++) {
printf("%f ", optimalValues[j]);
}
printf("\n");
}
}
//
for (int i = 0; i<optimalValues.size(); i++) {
optimalValues[i] /= numValues[i];
}
optimalSignal = vnl_vector<double>(&optimalValues[0], (int)optimalValues.size());
return true;
}
/*template <typename T1, typename T2>
itk::Image<T1,2>::Pointer vnlmat_to_itkimage(vnl_matrix<T2> mat,float scale = 1.0)
{
typedef typename itk::Image<T1,2> ImageType;
ImageType::SizeType size;
ImageType::IndexType index;
ImageType::RegionType region;
index.Fill(0);
size[0] = mat.cols();
size[1] = mat.rows();
region.SetSize(size);
region.SetIndex(index);
ImageType::Pointer im = ImageType::New();
im->SetRegions(region);
im->Allocate();
for(int xco = 0; xco<size[1]; xco++)
{
for(int yco = 0; yco < size[0]; yco++)
{
index[1] = xco;
index[0] = yco;
im->SetPixel(index,mat[xco][yco]*scale);
}
}
return im;
}
*/
vnl_matrix<float> cpx_to_abs(vnl_matrix< std::complex< float> > mat)
{
vnl_matrix<float> out(mat.rows(),mat.cols());
for(int x= 0; x< mat.rows(); x++)
{
for(int y = 0; y < mat.cols(); y++)
{
out[x][y] = abs(mat[x][y]);
}
}
return out;
bool rgrsn_ldp::local_viterbi(const vnl_matrix<double> & data,
double resolution,
const vnl_vector<double> & transition,
unsigned int window_size,
vnl_vector<double> & optimal_signal)
{
assert(resolution > 0.0);
assert(transition.size()%2 == 1);
const double min_v = data.min_value();
const double max_v = data.max_value();
const int nBin = (max_v - min_v)/resolution;
// raw data to probability map
// quantilization
const int N = data.rows();
vnl_matrix<double> probMap = vnl_matrix<double>(N, nBin);
for (int r = 0; r<N; r++) {
for (int c = 0; c<data.cols(); c++) {
int num = value_to_bin_number(min_v, resolution, data[r][c], nBin);
probMap[r][num] += 1.0;
}
}
probMap /= data.cols(); // normalization
vcl_vector<double> optimalValues(N, 0);
vcl_vector<int> numValues(N, 0); // multiple values from local dynamic programming
for (int i = 0; i <= N - window_size; i++) {
// get a local probMap;
vnl_matrix<double> localProbMap = probMap.extract(window_size, probMap.cols(), i, 0);
vcl_vector<int> localOptimalBins;
rgrsn_ldp::viterbi(localProbMap, transition, localOptimalBins);
assert(localOptimalBins.size() == window_size);
for (int j = 0; j < localOptimalBins.size(); j++) {
double value = bin_number_to_value(min_v, resolution, localOptimalBins[j]);
numValues[j + i] += 1;
optimalValues[j + i] += value;
}
}
// average all optimal path as final result
for (int i = 0; i<optimalValues.size(); i++) {
optimalValues[i] /= numValues[i];
}
optimal_signal = vnl_vector<double>(&optimalValues[0], (int)optimalValues.size());
return true;
}
/// \brief Unscented transform of process Sigma points
void UnscentedKalmanFilter::utf(vnl_matrix<double> X, vnl_vector<double> u,
vnl_vector<double> &y, vnl_matrix<double> &Y, vnl_matrix<double> &P, vnl_matrix<double> &Y1)
{
// determine number of sigma points
unsigned int L = X.cols();
// zero output matrices
y.fill(0.0); Y.fill(0.0);
// transform the sigma points and put them as columns in a matrix Y
for( int k = 0; k < L; k++ )
{
vnl_vector<double> xk = X.get_column(k);
vnl_vector<double> yk(N);
f(xk,u,yk);
Y.set_column(k,yk);
// add each transformed point to the weighted mean
y = y + Wm.get(0,k)*yk;
}
// create a matrix with each column being the weighted mean
vnl_matrix<double> Ymean(N,L);
for( int k = 0; k < L; k++ )
Ymean.set_column(k,y);
// set the matrix of difference vectors
Y1 = Y-Ymean;
// calculate the covariance matrix output
vnl_matrix<double> WC(L,L,0.0);
WC.set_diagonal(Wc.get_row(0));
P = Y1*WC*Y1.transpose();
}
bool rgrsn_ldp::transition_matrix(const vcl_vector<int> & fns,
const vcl_vector<double> & values,
vnl_matrix<double> & transition,
const double resolution)
{
assert(fns.size() == values.size());
double min_v = *vcl_min_element(values.begin(), values.end());
double max_v = *vcl_max_element(values.begin(), values.end());
unsigned num_bin = (max_v - min_v)/resolution;
transition = vnl_matrix<double>(num_bin, num_bin, 0.0);
vnl_vector<double> column(num_bin, 0.0);
for (int i = 0; i<fns.size()-1; i++) {
if (fns[i] + 1 == fns[i+1]) {
double cur_v = values[i];
double next_v = values[i+1];
unsigned cur_bin = value_to_bin_number(min_v, resolution, cur_v, num_bin);
unsigned next_bin = value_to_bin_number(min_v, resolution, next_v, num_bin);
transition[next_bin][cur_bin] += 1.0;
column[cur_bin] += 1.0;
}
}
// normalize each column
for (int r = 0; r < transition.rows(); r++) {
for (int c = 0; c < transition.cols(); c++) {
transition[r][c] /= column[c];
}
}
return true;
}
vnl_vector<double> MCLR_SM::Column_Order_Matrix(vnl_matrix<double> mat)
{
vnl_vector<double> mat_column_ordered;
mat_column_ordered.set_size(mat.rows()*mat.cols());
int count = 0;
for(int j=0;j<mat.cols();++j)
{
for(int i=0;i<mat.rows();++i)
{
mat_column_ordered(count) = mat(i,j);
count++;
}
}
return mat_column_ordered;
}
/** Applies an affine transform specified by the homogeneous transformation matrix to a vtkPolyData */
void psciob::AffineTransformVTKPolyData( vtkPolyData* inputPolyData, vnl_matrix<double> transformMatrix, vtkPolyData* outputPolyData) {
if ( transformMatrix.rows() != transformMatrix.cols() ) throw DeformableModelException("transform matrix must be square");
unsigned int d = transformMatrix.rows()-1;
vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New();
vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
switch(d) {
case 2:
matrix->SetElement(0,0, transformMatrix(0,0)); matrix->SetElement(0,1, transformMatrix(0,1)); matrix->SetElement(0,2, 0); matrix->SetElement(0,3, transformMatrix(0,2));
matrix->SetElement(1,0, transformMatrix(1,0)); matrix->SetElement(1,1, transformMatrix(1,1)); matrix->SetElement(1,2, 0); matrix->SetElement(1,3, transformMatrix(1,2));
matrix->SetElement(2,0, 0 ); matrix->SetElement(2,1, 0 ); matrix->SetElement(2,2, 1); matrix->SetElement(2,3, 0 );
matrix->SetElement(3,0, 0 ); matrix->SetElement(3,1, 0 ); matrix->SetElement(3,2, 0); matrix->SetElement(3,3, 1 );
break;
case 3:
for (unsigned i=0 ; i<4 ; i++) {
for (unsigned j=0 ; j<4 ; j++) {
matrix->SetElement(i,j, transformMatrix(i,j));
}
}
break;
default: throw DeformableModelException("only 2D or 3D transforms are allowed -> 3*3 or 4*4 matrices");
}
transform->SetMatrix( matrix );
transform->Update();
transformFilter->SetInput( inputPolyData );
transformFilter->SetTransform( transform );
transformFilter->Update();
outputPolyData->ShallowCopy( transformFilter->GetOutput() );
}
void denormalize(vnl_matrix<double>& x, const vnl_vector<double>& centroid, const double scale) {
int n = x.rows();
if (n==0) return;
int d = x.cols();
for (int i = 0; i < n; ++i) {
x.set_row(i, x.get_row(i) * scale + centroid);
}
}
vnl_matrix<double> MCLR_SM::Add_Bias(vnl_matrix<double> data)
{
vnl_matrix<double> data_with_bias; // Data to be modified
vnl_vector<double> temp_vector;
temp_vector.set_size(data.cols());
temp_vector.fill(1);
data_with_bias.set_size(no_of_features+1,data.cols());
data_with_bias.set_row(0,temp_vector);
for(int i =0; i<no_of_features ; ++i)
{
data_with_bias.set_row(i+1,data.get_row(i));
}
return data_with_bias;
}
double GaussTransform(const vnl_matrix<double>& A,
const vnl_matrix<double>& B, double scale,
vnl_matrix<double>& gradient) {
// assert A.cols() == B.cols()
return GaussTransform(A.data_block(), B.data_block(),
A.rows(), B.rows(), A.cols(), scale,
gradient.data_block());
}
void normalize_same(vnl_matrix<double>& x,
vnl_vector<double>& centroid, double& scale) {
int n = x.rows();
if (n==0) return;
int d = x.cols();
for (int i = 0; i < n; ++i) {
x.set_row(i, (x.get_row(i) - centroid) / scale);
}
}
int select_points(const vnl_matrix<double>&pts,
const std::vector<int>&index, vnl_matrix<double>& selected) {
int n = index.size();
int d = pts.cols();
selected.set_size(n,d);
for (int i = 0; i < n; ++i) {
selected.update(pts.extract(1, d, index[i]), i);
}
return n;
}
void vnl_flann::search(const vnl_matrix<double> & query_data,
vcl_vector<vcl_vector<int> > & indices,
vcl_vector<vcl_vector<double> > & dists, int knn) const
{
const int dim = (int)query_data.cols();
assert(dim == dim_);
Matrix<double> query_data_wrap((double *)query_data.data_block(), (int)query_data.rows(), dim);
index_.knnSearch(query_data_wrap, indices, dists, knn, flann::SearchParams(128));
}
// todo: add one more version when the model is same as ctrl_pts
// reference: Landmark-based Image Analysis, Karl Rohr, p195
void ComputeTPSKernel(const vnl_matrix<double>& model,
const vnl_matrix<double>& ctrl_pts,
vnl_matrix<double>& U, vnl_matrix<double>& K) {
int m = model.rows();
int n = ctrl_pts.rows();
int d = ctrl_pts.cols();
//asssert(model.cols()==d==(2|3));
K.set_size(n, n);
K.fill(0);
U.set_size(m, n);
U.fill(0);
double eps = 1e-006;
vnl_vector<double> v_ij;
for (int i = 0; i < m; ++i) {
for (int j = 0; j < n; ++j) {
v_ij = model.get_row(i) - ctrl_pts.get_row(j);
if (d == 2) {
double r = v_ij.squared_magnitude();
if (r > eps) {
U(i, j) = r * log(r) / 2;
}
} else if (d == 3) {
double r = v_ij.two_norm();
U(i, j) = -r;
}
}
}
for (int i = 0; i < n; ++i) {
for (int j = i + 1; j < n; ++j) {
v_ij = ctrl_pts.get_row(i) - ctrl_pts.get_row(j);
if (d == 2) {
double r = v_ij.squared_magnitude();
if (r > eps) {
K(i, j) = r * log(r) / 2;
}
}
else if (d == 3) {
double r = v_ij.two_norm();
K(i, j) = -r;
}
}
}
for (int i = 0; i < n; ++i) {
for (int j = 0; j < i; ++j) {
K(i, j) = K(j, i);
}
}
}
void itk::BiExpFitFunctor::operator()(vnl_matrix<double> & newSignal,const vnl_matrix<double> & SignalMatrix, const double & S0)
{
vnl_vector<double> initalGuess(3);
// initialize Least Squres Function
// SignalMatrix.cols() defines the number of shells points
lestSquaresFunction model(SignalMatrix.cols());
model.set_bvalues(m_BValueList);// set BValue Vector e.g.: [1000, 2000, 3000] <- shell b Values
// initialize Levenberg Marquardt
vnl_levenberg_marquardt minimizer(model);
minimizer.set_max_function_evals(1000); // Iterations
minimizer.set_f_tolerance(1e-10); // Function tolerance
// for each Direction calculate LSF Coeffs ADC & AKC
for(unsigned int i = 0 ; i < SignalMatrix.rows(); i++)
{
model.set_measurements(SignalMatrix.get_row(i));
model.set_reference_measurement(S0);
initalGuess.put(0, 0.f); // ADC_slow
initalGuess.put(1, 0.009f); // ADC_fast
initalGuess.put(2, 0.7f); // lambda
// start Levenberg-Marquardt
minimizer.minimize_without_gradient(initalGuess);
const double & ADC_slow = initalGuess.get(0);
const double & ADC_fast = initalGuess.get(1);
const double & lambda = initalGuess(2);
newSignal.put(i, 0, S0 * (lambda * std::exp(-m_TargetBvalue * ADC_slow) + (1-lambda)* std::exp(-m_TargetBvalue * ADC_fast)));
newSignal.put(i, 1, minimizer.get_end_error()); // RMS Error
//OUTPUT FOR EVALUATION
/*std::cout << std::scientific << std::setprecision(5)
<< ADC_slow << "," // lambda
<< ADC_fast << "," // alpha
<< lambda << "," // lambda
<< S0 << "," // S0 value
<< minimizer.get_end_error() << ","; // End error
for(unsigned int j = 0; j < SignalMatrix.get_row(i).size(); j++ ){
std::cout << std::scientific << std::setprecision(5) << SignalMatrix.get_row(i)[j]; // S_n Values corresponding to shell 1 to shell n
if(j != SignalMatrix.get_row(i).size()-1) std::cout << ",";
}
std::cout << std::endl;*/
}
}
bool rgrsn_ldp::compact_transition_matrix(const vcl_vector<int> & fns,
const vcl_vector<double> & values,
vnl_matrix<double> & transition,
const double resolution)
{
assert(fns.size() == values.size());
double min_v = *vcl_min_element(values.begin(), values.end());
double max_v = *vcl_max_element(values.begin(), values.end());
unsigned num_bin = (max_v - min_v)/resolution;
unsigned max_bin_transition = 0; // biggest transition between frames quantized in bin
for (int i = 0; i<fns.size(); i++) {
if (fns[i] + 1 == fns[i+1]) {
double cur_v = values[i];
double next_v = values[i+1];
int cur_bin = value_to_bin_number(min_v, resolution, cur_v, num_bin);
int next_bin = value_to_bin_number(min_v, resolution, next_v, num_bin);
int dif = abs(next_bin - cur_bin);
if (dif > max_bin_transition) {
max_bin_transition = dif;
}
}
}
transition = vnl_matrix<double>(max_bin_transition * 2 + 1, num_bin, 0.0);
vnl_vector<double> column(num_bin, 0.0);
for (int i = 0; i<fns.size()-1; i++) {
if (fns[i] + 1 == fns[i+1]) {
double cur_v = values[i];
double next_v = values[i+1];
int cur_bin = value_to_bin_number(min_v, resolution, cur_v, num_bin);
int next_bin = value_to_bin_number(min_v, resolution, next_v, num_bin);
int row = max_bin_transition + (next_bin - cur_bin);
transition[row][cur_bin] += 1.0;
column[cur_bin] += 1.0;
}
}
// normalize each column
for (int r = 0; r < transition.rows(); r++) {
for (int c = 0; c < transition.cols(); c++) {
// transition[r][c] /= column[c];
}
}
return true;
}
int vnl_matrix2vtkPolyData(vtkPolyData* A, vnl_matrix<double>& matrix)
{
int dim = matrix.cols();;
int n = matrix.rows();
//int n = A->GetNumberOfPoints();
//matrix.set_size(n, dim);
double P[3];
for(int i = 0;i < n; i++)
{
for(int j = 0;j < dim; j++ )
{
P[j] = matrix(i,j);
}
A->GetPoints()->SetPoint(i,P);
}
return 1;
}
void compute_P(const vnl_matrix<double>& x,const vnl_matrix<double>& y, vnl_matrix<double>& P, double &E, double sigma, int outliers) {
double k;
k = -2*sigma*sigma;
//P.set_size(m,n); P.fill(0);
//vnl_vector<double> v_ij;
vnl_vector<double> column_sum;
int m = x.rows();
int s = y.rows();
int d = x.cols();
column_sum.set_size(s);
column_sum.fill(0);
double outlier_term = outliers*pow((2*sigma*sigma*3.1415926),0.5*d);
for (int i = 0; i < m; ++i) {
for (int j = 0; j < s; ++j) {
double r = 0;
for (int t = 0; t < d; ++t) {
r += (x(i,t) - y(j,t))*(x(i,t) - y(j,t));
}
P(i,j) = exp(r/k);
column_sum[j]+=P(i,j);
}
}
if (outliers!=0) {
for (int i = 0; i < s; ++i)
column_sum[i] += outlier_term;
}
if (column_sum.min_value()>(1e-12)) {
E = 0;
for (int i = 0; i < s; ++i) {
for (int j = 0; j < m; ++j){
P(j,i) = P(j,i)/column_sum[i];
}
E -= log(column_sum[i]);
}
//vcl_cerr< < s;
//vcl_cerr<<P.get_column(10);
}
else {
P.empty();
}
}
void normalize(vnl_matrix<double>& x,
vnl_vector<double>& centroid, double& scale) {
int n = x.rows();
if (n==0) return;
int d = x.cols();
centroid.set_size(d);
vnl_vector<double> col;
for (int i = 0; i < d; ++i) {
col = x.get_column(i);
centroid(i) = col.mean();
}
for (int i = 0; i < n; ++i) {
x.set_row(i, x.get_row(i) - centroid);
}
scale = x.frobenius_norm() / sqrt(double(n));
x = x / scale;
}
void f(const vnl_matrix<double>& model,
const vnl_matrix<double>& scene, double threshold,
vnl_matrix<double>& extracted_model,
vnl_matrix<double>& extracted_scene) {
vnl_matrix<double> dist;
vnl_matrix<int> pairs;
ComputeSquaredDistanceMatrix(model, scene, dist);
pick_indices(dist, pairs, threshold*threshold);
std::cout << "distance threshold : " << threshold << std::endl;
int j, n = pairs.cols();
int d = model.cols();
extracted_model.set_size(n,d);
extracted_scene.set_size(n,d);
std::cout << "# of matched point pairs : " << n << std::endl;
for (j=0; j<n; ++j) {
extracted_model.set_row(j,model.get_row(pairs(0,j)));
}
for (j=0; j<n; ++j) {
extracted_scene.set_row(j,scene.get_row(pairs(1,j)));
}
}
void mitk::GeneralizedLinearModel::EstimatePermutation(const vnl_matrix<double> &xData)
{
v3p_netlib_integer rows = xData.rows();
v3p_netlib_integer cols = xData.cols();
if (m_AddConstantColumn)
++cols;
v3p_netlib_doublereal *x = new v3p_netlib_doublereal[rows* cols];
_UpdateXMatrix(xData, m_AddConstantColumn, x);
v3p_netlib_doublereal *qraux = new v3p_netlib_doublereal[cols];
v3p_netlib_integer *jpvt = new v3p_netlib_integer[cols];
std::fill_n(jpvt,cols,0);
v3p_netlib_doublereal *work = new v3p_netlib_doublereal[cols];
std::fill_n(work,cols,0);
v3p_netlib_integer job = 16;
// Make a call to Lapack-DQRDC which does QR with permutation
// Permutation is saved in JPVT.
v3p_netlib_dqrdc_(x, &rows, &rows, &cols, qraux, jpvt, work, &job);
double limit = std::abs(x[0]) * std::max(cols, rows) * std::numeric_limits<double>::epsilon();
// Calculate the rank of the matrix
int m_Rank = 0;
for (int i = 0; i <cols; ++i)
{
m_Rank += (std::abs(x[i*rows + i]) > limit) ? 1 : 0;
}
// Create a permutation vector
m_Permutation.set_size(m_Rank);
for (int i = 0; i < m_Rank; ++i)
{
m_Permutation(i) = jpvt[i]-1;
}
delete x;
delete qraux;
delete jpvt;
delete work;
}
void ExtractMatchingPairs(
const vnl_matrix<T>& model,
const vnl_matrix<T>& scene,
const T& threshold,
vnl_matrix<T>& extracted_model,
vnl_matrix<T>& extracted_scene) {
vnl_matrix<T> dist;
vnl_matrix<int> pairs;
ComputeSquaredDistanceMatrix<T>(model, scene, dist);
PickIndices<T>(dist, pairs, threshold*threshold);
std::cout << "distance threshold : " << threshold << std::endl;
int n = pairs.cols();
int d = model.cols();
extracted_model.set_size(n, d);
extracted_scene.set_size(n, d);
std::cout << "# of matched point pairs : " << n << std::endl;
for (int j = 0; j < n; ++j) {
extracted_model.set_row(j,model.get_row(pairs(0, j)));
}
for (int j = 0; j < n; ++j) {
extracted_scene.set_row(j,scene.get_row(pairs(1, j)));
}
}
void ComputeTPSKernelU(const vnl_matrix<double>& model,
const vnl_matrix<double>& ctrl_pts,
vnl_matrix<double>& U) {
int m = model.rows();
int n = ctrl_pts.rows();
int d = ctrl_pts.cols();
//asssert(model.cols()==d==(2|3));
//K.set_size(n, n);
//K.fill(0);
U.set_size(m, n);
U.fill(0);
double eps = 1e-006;
vnl_vector<double> v_ij;
for (int i = 0; i < m; ++i) {
for (int j = 0; j < n; ++j)
{
v_ij = model.get_row(i) - ctrl_pts.get_row(j);
double r = v_ij.two_norm();
U(i, j) = -r;
}
}
}
/*
Matlab code in cpd_G.m:
k=-2*beta^2;
[n, d]=size(x); [m, d]=size(y);
G=repmat(x,[1 1 m])-permute(repmat(y,[1 1 n]),[3 2 1]);
G=squeeze(sum(G.^2,2));
G=G/k;
G=exp(G);
*/
void ComputeGaussianKernel(const vnl_matrix<double>& model,
const vnl_matrix<double>& ctrl_pts,
vnl_matrix<double>& G, vnl_matrix<double>& K,
double lambda) {
int m,n,d;
m = model.rows();
n = ctrl_pts.rows();
d = ctrl_pts.cols();
//asssert(model.cols()==d);
//assert(lambda>0);
G.set_size(m,n);
GaussianAffinityMatrix(model.data_block(), ctrl_pts.data_block(),
m, n, d, lambda, G.data_block());
if (model == ctrl_pts) {
K = G;
} else {
K.set_size(n,n);
GaussianAffinityMatrix(ctrl_pts.data_block(), ctrl_pts.data_block(),
n, n, d, lambda, K.data_block());
}
}
//Reshape the matrix : columns first ; Similar to MATLAB
vnl_matrix<double> MCLR_SM::Reshape_Matrix(vnl_matrix<double>mat,int r,int c )
{
if(mat.rows()*mat.cols() != r*c)
{
cout<< "Number of elements in the matrix/vector should be equal to the total number of elements in the reshaped matrix";
getchar();
exit(1);
}
vnl_matrix<double>reshaped_matrix;
reshaped_matrix.set_size(r,c);
int count = 0;
for(int j=0;j<c;++j)
{
for(int i=0;i<r;++i)
{
reshaped_matrix(i,j) = mat(count%mat.rows(),floor(static_cast<double>(count/mat.rows())));
count++;
}
}
return reshaped_matrix;
}
// Copy a vnl-matrix to an c-array with row-wise representation.
// Adds a constant column if required.
static void _UpdateXMatrix(const vnl_matrix<double> &xData, bool addConstant, v3p_netlib_doublereal *x)
{
v3p_netlib_integer rows = xData.rows();
v3p_netlib_integer cols = xData.cols();
if (addConstant)
++cols;
for (int r=0; r < rows; ++r)
{
for (int c=0; c <cols; ++c)
{
if (!addConstant)
{
x[c*rows + r] = xData(r,c);
} else if (c == 0)
{
x[c*rows + r] = 1.0;
} else
{
x[c*rows + r] = xData(r, c-1);
}
}
}
}
bool rgrsn_ldp::dynamic_programming(const vnl_matrix<double> & data,
double v_min, double v_max,
unsigned int nBin,
int nJumpBin,
unsigned int windowSize,
vnl_vector<double> & optimalSignal,
vnl_vector<double> & signal_variance)
{
assert(v_min < v_max);
// raw data to probability map
// quantilization
const int N = data.rows();
vnl_matrix<double> probMap = vnl_matrix<double>(N, nBin);
double interval = (v_max - v_min)/nBin;
for (int r = 0; r<N; r++) {
for (int c = 0; c<data.cols(); c++) {
int num = value_to_bin_number(v_min, interval, data[r][c], nBin);
probMap[r][num] += 1.0;
}
}
probMap /= data.cols(); // normalization
vcl_vector<double> optimalValues(N, 0);
vcl_vector<int> numValues(N, 0); // multiple values from local dynamic programming
vcl_vector<vcl_vector<double> > all_values(N);
for (int i = 0; i<=N - windowSize; i++) {
// get a local probMap;
vnl_matrix<double> localProbMap = probMap.extract(windowSize, probMap.cols(), i, 0);
vcl_vector<int> localOptimalBins;
rgrsn_ldp::local_dynamic_programming(localProbMap, nJumpBin, localOptimalBins);
assert(localOptimalBins.size() == windowSize);
for (int j = 0; j < localOptimalBins.size(); j++) {
double value = bin_number_to_value(v_min, interval, localOptimalBins[j]);
assert(j + i < N);
all_values[j + i].push_back(value);
numValues[j + i] += 1;
optimalValues[j + i] += value;
}
}
// mean value
for (int i = 0; i<optimalValues.size(); i++) {
optimalValues[i] /= numValues[i];
}
// variance
signal_variance = vnl_vector<double>(N, 0);
for (int i = 0; i<optimalValues.size(); i++) {
assert(all_values[i].size() > 0);
if (all_values[i].size() == 1) {
signal_variance[i] = 0.0001;
}
else
{
double dump_mean = 0.0;
double sigma = 0.0;
VnlPlus::mean_std(&all_values[i][0], (int)all_values[i].size(), dump_mean, sigma);
signal_variance[i] = sigma + 0.0001; // avoid zero
}
}
optimalSignal = vnl_vector<double>(&optimalValues[0], (int)optimalValues.size());
// save variance with the size of window size, for test purpose
if(0)
{
vcl_vector<vnl_vector<double> > all_value_vecs;
for (int i = 0; i<all_values.size(); i++) {
if (all_values[i].size() == windowSize) {
all_value_vecs.push_back(VnlPlus::vector_2_vec(all_values[i]));
}
}
vcl_string save_file("ldp_all_prediction.mat");
vnl_matlab_filewrite awriter(save_file.c_str());
awriter.write(VnlPlus::vector_2_mat(all_value_vecs), "ldp_all_opt_path");
printf("save to %s\n", save_file.c_str());
}
return true;
}
