bool fit_lm_solver::solve(fit_function *fn,vnl_vector<double> &x){
//fn->lsqf_;
//data:
vnl_levenberg_marquardt levmarq( * fn->lsqf_ );
//vnl_vector<double> xi=fn->xi_;
//vnl_vector<double> y=fn->y_;
// levmarq.set_verbose(true);
levmarq.set_x_tolerance(1e-10);
levmarq.set_epsilon_function(1);
levmarq.set_f_tolerance(1e-10);
levmarq.set_max_function_evals(50);
vnl_vector<double> params(x.size());
//initial_values(xi, y, params);
fn->lsif_->init(params);
// Minimize the error and get the best intersection point
levmarq.minimize(params);
levmarq.diagnose_outcome();
//double t;
for (unsigned i=0;i<x.size();i++)
{
x[i]=params[i];
// t=x[i];
}
if (abs(fn->lsqf_->residual)> 1e-10)
return false;
return true;
}
bool fit_lm_solver::retry(fit_function *fn,vnl_vector<double>&x,unsigned n_iter,double xtol,double ftol ){
cout<<"Failed first time. then will try again using n_iter= "<<n_iter
<<" xtol="<<xtol
<<" ftol="<<ftol<<endl;
vnl_levenberg_marquardt levmarq( * fn->lsqf_ );
vnl_vector<double> xi=fn->xi_;
vnl_vector<double> y=fn->y_;
levmarq.set_verbose(true);
levmarq.set_x_tolerance(xtol);
levmarq.set_epsilon_function(1);
levmarq.set_f_tolerance(ftol);
levmarq.set_max_function_evals(n_iter);
vnl_vector<double> params(x.size());
//initial_values(xi, y, params);
fn->lsif_->init(params);
// Minimize the error and get the best intersection point
levmarq.minimize(params);
levmarq.diagnose_outcome();
for (unsigned i=0;i<x.size();i++)
{
x[i]=params[i];
}
return true;
}
vnl_vector<double> psciob::TransformBoundingBox(const vnl_vector<double> &inputBB, const vnl_matrix<double> &transformMatrix) {
unsigned int n = transformMatrix.rows();
unsigned int len = inputBB.size();
unsigned int D = n-1;
if ( (n!=transformMatrix.rows()) || (len!=2*D) ) {
std::cout<<"-- size inputBB= "<<inputBB.size()<<", size transformMatrix= "<<transformMatrix.rows()<<" ; matrix = "<<transformMatrix<<std::endl;
throw DeformableModelException("Error in TransformBoundingBox : inconsistent inputs");
}
vnl_vector<double> outputBB(len);
vnl_matrix<double> rot(D,D); vnl_vector<double> trans(D);
rot = transformMatrix.extract(D,D,0,0); trans = transformMatrix.extract(D,1,0,D).get_column(0);
vnl_vector<double> p1(D), p2(D), p3(D), p4(D), p5(D), p6(D), p7(D), p8(D);
vnl_vector<double> q1(D), q2(D), q3(D), q4(D), q5(D), q6(D), q7(D), q8(D);
switch(D) {
case 2:
p1(0) = inputBB(0); p1(1) = inputBB(2); p2(0) = inputBB(1); p2(1) = inputBB(2);
p3(0) = inputBB(1); p3(1) = inputBB(3); p4(0) = inputBB(0); p4(1) = inputBB(3);
q1 = rot*p1 + trans; q2 = rot*p2 + trans; q3 = rot*p3 + trans; q4 = rot*p4 + trans;
outputBB(0)= q1(0); outputBB(1)= q1(0); outputBB(2)= q1(1); outputBB(3)= q1(1);
for (unsigned int i=0 ; i<D ; i++) {
outputBB(2*i) = min(outputBB(2*i), q2(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q2(i));
outputBB(2*i) = min(outputBB(2*i), q3(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q3(i));
outputBB(2*i) = min(outputBB(2*i), q4(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q4(i));
}
break;
case 3:
p1(0) = inputBB(0); p1(1) = inputBB(2); p1(2) = inputBB(4); p2(0) = inputBB(1); p2(1) = inputBB(2); p2(2) = inputBB(4);
p3(0) = inputBB(1); p3(1) = inputBB(3); p3(2) = inputBB(4); p4(0) = inputBB(0); p4(1) = inputBB(3); p4(2) = inputBB(4);
p5(0) = inputBB(0); p5(1) = inputBB(2); p5(2) = inputBB(5); p6(0) = inputBB(1); p6(1) = inputBB(2); p6(2) = inputBB(5);
p7(0) = inputBB(1); p7(1) = inputBB(3); p7(2) = inputBB(5); p8(0) = inputBB(0); p8(1) = inputBB(3); p8(2) = inputBB(5);
q1 = rot*p1 + trans; q2 = rot*p2 + trans; q3 = rot*p3 + trans; q4 = rot*p4 + trans;
q5 = rot*p5 + trans; q6 = rot*p6 + trans; q7 = rot*p7 + trans; q8 = rot*p8 + trans;
outputBB(0)= q1(0); outputBB(1)= q1(0); outputBB(2)= q1(1); outputBB(3)= q1(1); outputBB(4)= q1(2); outputBB(5)= q1(2);
for (unsigned int i=0 ; i<D ; i++) {
outputBB(2*i) = min(outputBB(2*i), q2(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q2(i));
outputBB(2*i) = min(outputBB(2*i), q3(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q3(i));
outputBB(2*i) = min(outputBB(2*i), q4(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q4(i));
outputBB(2*i) = min(outputBB(2*i), q5(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q5(i));
outputBB(2*i) = min(outputBB(2*i), q6(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q6(i));
outputBB(2*i) = min(outputBB(2*i), q7(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q7(i));
outputBB(2*i) = min(outputBB(2*i), q8(i)); outputBB(2*i+1) = max(outputBB(2*i+1), q8(i));
}
break;
default :
throw DeformableModelException("Error in TransformBoundingBox : expecting 2 or 3 dimensional spaces only");
break;
}
return outputBB;
}
std::vector<double> QmitkIVIMWidget::vec(const vnl_vector<double>& vector)
{
std::vector<double> retval(vector.size());
for(unsigned int i=0; i<vector.size(); i++)
{
retval.at(i) = vector[i];
}
return retval;
}
std::vector<double> QmitkODFDetailsWidget::vec(vnl_vector<double> vector)
{
std::vector<double> retval(vector.size());
for(unsigned int i=0; i<vector.size(); i++)
{
retval.at(i) = vector[i];
}
return retval;
}
vnl_matrix<double> MCLR_SM::Kron(vnl_vector<double> x,vnl_vector<double> y )
{
vnl_matrix<double> q(x.size()*y.size(),1);
int counter = 0;
for(int j = 0; j < x.size() ; ++j)
for(int k = 0; k < y.size() ; ++k)
{
q(counter,0) = x(j)*y(k);
counter++;
}
return q;
}
void getDynamicBackgroundSumLogProb(IplImage *smooth,
const vnl_vector<FLOAT> &bgProb,
vnl_vector<FLOAT> &sumPix,
FLOAT &sum)
{
unsigned int widthExtra=smooth->width+1; // each line needs an extra leading zero
unsigned int imgSizeExtra=widthExtra*smooth->height;
if (sumPix.size()!=imgSizeExtra)
sumPix.set_size(imgSizeExtra);
int pixelInd=0,channelInd=0;
sum=0;
for (unsigned int i=0;i<imgSizeExtra;i++)
{
if (i%widthExtra==0) // add leading zero
sumPix(i)=0;
else
{
sumPix(i)=sumPix(i-1)+bgProb(pixelInd)+3.0*log(256); // - (-log ...) , something to do with foreground probablity
sum+=bgProb(pixelInd);
pixelInd+=1;channelInd+=3; // next pixel
}
}
}
//Check Parameters
inline
bool Fast2DEllipse::CheckParameters(const vnl_vector<double> &p) const {
if (p.size()!=m_nbParams) return false;
if (p(2)<TINY) return false; //no negative length
if ( (p(3)<TINY) || (p(3)>1) ) return false; // elongation must be in [0,1]
return true;
}
void OutputVNLVector(const vnl_vector<double> &V)
{
for(unsigned int i = 0; i < V.size(); i++)
std::cout << V[i] << " ";
std::cout << std::endl;
}
void blIALinearSampler3D::NormalizeProfile( vnl_vector<double>& profile, ProfileNormalizationType method )
{
double norm = 0;
switch( method )
{
case normZeroMeanUnitVar:
{
const double mean = profile.mean();
profile = profile - mean;
const double var = profile.squared_magnitude()/(profile.size()-1); //unbiased
if( var==0 && profile[0]!=0 ) profile.normalize();
else if( var!=0 ) profile /= sqrt(var);
}
break;
case normZeroMeanL1:
profile = profile - profile.mean();
norm = profile.one_norm();
if( norm!=0 ) profile /= profile.one_norm();
break;
case normL1:
norm = profile.one_norm();
if( norm!=0 ) profile /= profile.one_norm();
break;
case normNone:
break;
default:
std::cerr<<"Unknown profile normalization method"<<std::endl;
throw "Unknown profile normalization method";
}
}
bool arlCore::FieldCorrector::setParameters( const vnl_vector<double> ¶meters )
{
if(parameters.size()!=getNbParameters())
{
const unsigned int NbEquations = 3;
unsigned int degree = 1;
while(nbPolynomialParameters(degree, NbEquations)!=parameters.size() && degree<10)
{
++degree;
}
if(nbPolynomialParameters(degree, NbEquations)!=parameters.size()) return false;
setDegree(degree);
}
m_parameters = parameters;
return true;
}
std::vector<double> vnl_vector_to_vector(const vnl_vector<double> &v)
{
std::vector<double> vec;
for(unsigned int i = 0; i < v.size(); i++)
vec.push_back(v(i));
return vec;
}
bool CloseEnough(const vnl_vector<double> &v1, const vnl_vector<double> &v2, const double eps)
{
for(unsigned int i = 0; i < v1.size(); i++)
{
if(fabs(v1[i] - v2[i]) > eps)
return false;
}
return true;
}
string fit_lm_solver::outcome(vnl_vector<double> const&x){
string r ="fit_lm_solver";
cout<< "*********fit_lm_solver,parameter shows below:***********"<<endl;
for (unsigned i=0;i<x.size();i++)
{
cout<<" "<<x[i]<<" "<<endl;
}
return r;
}
// get the covariance of a contrast given a contrast vector
// if this matrix is X, this function computes c' pinv(X'X) c
double RtDesignMatrix::computeContrastCovariance(
vnl_vector<double> &contrastVector) {
if (contrastVector.size() != columns()) {
cerr << "ERROR: number of elements in contrast vector does not match the "
<< "number of columns in the design matrix" << endl;
return std::numeric_limits<double>::quiet_NaN();
}
// compute the contrast covariance based on the currently known regressors
// NOTE: this will not be the same as computing it at the end of the
// experiment when all regressors are known. it would be nice to compute
// final values using the known design.
vnl_matrix<double> convec(contrastVector.data_block(),
contrastVector.size(), 1);
vnl_svd<double> pinv(transpose() * (*this));
vnl_matrix<double> result = convec.transpose() * pinv.pinverse() * convec;
return result.get(0, 0);
}
// Inverts the permutation on a given b-vector.
// Necessary to get a b-vector that match the original data
static void _FinalizeBVector(vnl_vector<double> &b, vnl_vector<unsigned int> &perm, int cols)
{
vnl_vector<double> tempB(cols+1);
tempB.fill(0);
for (unsigned int c = 0; c < perm.size(); ++c)
{
tempB(perm(c)) = b(c);
}
b = tempB;
}
// Initialize mu and eta by calling the corresponding
// link and distribution functions on each row
static void _InitMuEta(mitk::DistSimpleBinominal *dist, mitk::LogItLinking *link, const vnl_vector<double> &yData, vnl_vector<double> &mu, vnl_vector<double> &eta)
{
int rows = yData.size();
mu.set_size(rows);
eta.set_size(rows);
for (int r = 0; r < rows; ++r)
{
mu(r) = dist->Init(yData(r));
eta(r) = link->Link(mu(r));
}
}
// convolves a vector with the hrf for this design
vnl_vector<double> RtDesignMatrix::convolveVecWithHrf(
const vnl_vector<double> &v) {
vnl_vector<double> conHiresV(v.size() / hrfSamplePeriod);
// upsample
double curTr = 0;
for (unsigned int i = 0; i < conHiresV.size();
i++, curTr += hrfSamplePeriod) {
conHiresV.put(i, v[(int) floor(curTr)]);
}
// convolve
vnl_vector<double> tmp = vnl_convolve(conHiresV, getHiresHrf());
conHiresV.update(tmp.extract(conHiresV.size()));
// downsample
vnl_vector<double> conV(v.size(), 0);
for (unsigned int i = 0; i < conV.size(); i++) {
conV.put(i, conHiresV[i / hrfSamplePeriod]);
}
return conV;
}
// loads an hrf vector from a file
bool RtDesignMatrix::loadHrfFile(vnl_vector<double> &hrf, string filename) {
vcl_ifstream in(filename.c_str(), ios::in);
if (in.fail()) {
return false;
}
if (!hrf.read_ascii(in)) {
return false;
}
// compute other parms
hrfSamplePeriod = tr;
hiresHrf = hrf;
// compute the temporal derivative basis
temporalDerivative.set_size(hrf.size());
temporalDerivative.fill(0);
temporalDerivative.update(hrf.extract(hrf.size() - 1, 1), 1);
temporalDerivative = hrf - temporalDerivative;
hiresTemporalDerivative = temporalDerivative;
return true;
}
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;
}
bool LinearCombinationPDF::SetParameters(const vnl_vector<double> &p) {
if (p.size()!=m_nbP_a+m_nbP_b+m_nbP_x) return false;
//vnl_vector<double> backupParams = m_params;
if (!m_x->SetParameters(p.extract(m_nbP_x,0))) return false;
if (!m_a->SetParameters(p.extract(m_nbP_a, m_nbP_x))) { //undo the modifications of the previous pdfs
if (!m_x->SetParameters(m_params.extract(m_nbP_x,0))) throw DeformableModelException("LinearCombinationPDF::SetParameters : trying to set invalid parameters, and unable to rewind to previous parameters ; SHOULD NEVER HAPPEN!");
return false;
}
if (!m_b->SetParameters(p.extract(m_nbP_b, m_nbP_x+m_nbP_a))) { //undo the modifications of the previous pdfs
if (!m_a->SetParameters(m_params.extract(m_nbP_a, m_nbP_x))) throw DeformableModelException("LinearCombinationPDF::SetParameters : trying to set invalid parameters, and unable to rewind to previous parameters ; SHOULD NEVER HAPPEN!");
if (!m_x->SetParameters(m_params.extract(m_nbP_x,0))) throw DeformableModelException("LinearCombinationPDF::SetParameters : trying to set invalid parameters, and unable to rewind to previous parameters ; SHOULD NEVER HAPPEN!");
return false;
}
m_params = p;
return true;
}
bool UnivariateMixturePDF::SetParameters(const vnl_vector<double> &p) {
if (p.size()!=m_totalNbParams) return false;
unsigned cumSumNbParams=0;
//set the parameters of each law, in turn...
for (unsigned i=0 ; i<m_univ_pdfs.size() ; i++) {
if (!m_univ_pdfs[i]->SetParameters(p.extract(m_nbP[i] ,cumSumNbParams))) {
//rewind modifications if those parameters are invalid
for (int j=i-1 ; j>=0 ; j--) {
cumSumNbParams -= m_nbP[j];
if (!m_univ_pdfs[j]->SetParameters(m_params.extract(m_nbP[j] ,cumSumNbParams))) throw DeformableModelException("UnivariateMixturePDF::SetParameters : trying to set invalid parameters, and unable to rewind to previous parameters ; SHOULD NEVER HAPPEN!");
}
return false;
}
cumSumNbParams+=m_nbP[i];
}
m_params = p;
return true;
}
void getDynamicBackgroundLogProb(IplImage *smooth,
GaussianMixture<DYNBG_GAUS,DYNBG_TYPE,DYNBG_MAXGAUS> *gaussianMixtures,
vnl_vector<FLOAT> &bgProb)
{
unsigned int size=smooth->width*smooth->height;
if (bgProb.size()!=size)
bgProb.set_size(size);
bgProbMin=std::numeric_limits<float>::max();
bgProbMax=-std::numeric_limits<float>::max();
// threaded version
boost::thread threads[nrThreads];
for (unsigned i=0;i<nrThreads;i++)
{
unsigned begin=i*(size/nrThreads);
unsigned end=(i+1)*(size/nrThreads);
if (i==nrThreads-1) // is last
end=size;
threads[i]=boost::thread(DynBackGroundThread(begin,end,
smooth,gaussianMixtures,bgProb));
}
for (unsigned i=0;i<nrThreads;i++)
threads[i].join();
/* // non-threaded version
int updateGaussianID;
DYNBG_TYPE data[DYNBG_DIM],squareDist[DYNBG_DIM];
int channelInd=0;
for (unsigned int i=0;i<size;i++)
{
data[0]=(unsigned char)(smooth->imageData[channelInd+0]);
data[1]=(unsigned char)(smooth->imageData[channelInd+1]);
data[2]=(unsigned char)(smooth->imageData[channelInd+2]);
DYNBG_TYPE logProbBG=gaussianMixtures[i].logProbability(data,squareDist,minWeight,squareMahanobisMatch,updateGaussianID);
gaussianMixtures[i].update(data,initVar,decay,weightReduction,updateGaussianID);
bgProb(i)=logProbBG;
if (logProbBG<bgProbMin) bgProbMin=logProbBG;
if (logProbBG>bgProbMax) bgProbMax=logProbBG;
channelInd+=3; // next pixel
}
*/
}
void TpsRegistration::SetAffineAndTps(const vnl_vector<double>& x) {
/* reshape x, assuming x is row major; */
int rows_x = x.size() / d_;
if (func_->fix_affine_) { // affine is given, x does not include affine
param_all_.update(param_affine_);
for (int i = 0, k = 0; i < rows_x; ++i) {
for (int j = 0; j < d_; ++j, ++k) {
param_tps_(i, j) = x[k];
}
}
param_all_.update(param_tps_, d_ + 1);
} else { // affine is not given, x includes affine already
for (int i = 0, k = 0; i < rows_x; ++i) {
for (int j = 0; j < d_; ++j, ++k) {
param_all_(i, j) = x[k];
}
}
param_affine_ = param_all_.extract(d_ + 1, d_);
param_tps_ = param_all_.extract(rows_x - d_ - 1, d_, d_ + 1);
}
}
//Reshape the vector into a matrix : Similar to MATLAB
vnl_matrix<double> MCLR_SM::Reshape_Vector(vnl_vector<double>vec,int r,int c )
{
if(vec.size() != r*c)
{
std::cerr << "Number of elements in the matrix/vector should be equal to the total number of elements in the reshaped matrix";
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) = vec(count);
count++;
}
}
return reshaped_matrix;
}
// Fills the value of the xData-matrix into the x-matrix. Adds a constant
// column if required. Permutes the rows corresponding to the permutation vector.
static void _UpdatePermXMatrix(const vnl_matrix<double> &xData, bool addConstant, const vnl_vector<unsigned int> &permutation, vnl_matrix<double> &x)
{
int rows = xData.rows();
int cols = permutation.size();
x.set_size(rows, cols);
for (int r=0; r < rows; ++r)
{
for (int c=0; c<cols; ++c)
{
unsigned int newCol = permutation(c);
if (!addConstant)
{
x(r, c) = xData(r,newCol);
} else if (newCol == 0)
{
x(r, c) = 1.0;
} else
{
x(r, c) = xData(r, newCol-1);
}
}
}
}
vnl_matrix<double> Reshape(const vnl_vector<double> &V, const unsigned int rows, const unsigned int cols)
{
//This function reshapes a vector into a matrix using row major construction.
//check input sizes
if(V.size() != rows*cols)
{
std::cout << "Data sizes do not match!" << std::endl;
}
vnl_matrix<double> M(rows, cols);
unsigned int counter = 0;
for(unsigned int r = 0; r < rows; r++)
{
for(unsigned int c = 0; c < cols; c++)
{
M(r,c) = V[counter];
counter++;
}
}
return M;
}
void histogram(const vector<CvPoint> &tplt, IplImage *img, vnl_vector<double> &hist, unsigned iPerBin = 1)
{
vector<scanline_t> mask;
getMask(tplt,mask);
if (hist.size() == 0) {
hist.set_size(256/iPerBin);
hist.fill(0.0);
}
unsigned char *src = (unsigned char *)img->imageData;
unsigned K = img->nChannels, denom = K*iPerBin;
for (unsigned i=0; i<mask.size(); ++i) {
unsigned char *s=src+mask[i].line*img->widthStep + K*mask[i].start;
for (unsigned j=mask[i].start; j<mask[i].end; ++j) {
unsigned val=0.;
for (unsigned k=0; k!=K; ++k, ++s) {
val += *s;
*s = 0;
}
val /= denom;
hist[val]++;
}
}
}
double arlCore::ICP::computeCriterion( const arlCore::vnl_rigid_matrix &M, vnl_vector< double > &fx )
{
vnl_vector<double> RMS(1, 0.0), nbPoints(1, 0.0);
const arlCore::vnl_rigid_matrix InvM = M.computeInverse();
unsigned int i, j, noTriangle;
double n = 0.0, result = 0.0;
if(m_point2PointMode)
{ // Points to points
#ifdef ANN
const unsigned int Dimension = 3;
vnl_vector_fixed<double,3> traInit = InvM.getTranslation();
vnl_matrix_fixed<double,3,3> rotInit = InvM.getRotation();
const double Epsilon = 0.0;// Error bound
ANNpoint Pt = annAllocPt(Dimension); // Query point
for( i=0 ; i<m_cloudSize ; ++i )
{ // Search the matching point for every point of cloud
for( j=0 ; j<3; ++j )
Pt[j] = rotInit[j][0]*m_cloudPoints[i][0]+rotInit[j][1]*m_cloudPoints[i][1]+rotInit[j][2]*m_cloudPoints[i][2]+traInit[j];
m_ANNtree->annkSearch( Pt, m_nbNN, m_nn_idx, m_squaredDists, Epsilon );
if(fx.size()>i) fx[i] = m_squaredDists[0];
RMS[0] += m_squaredDists[0];
}
annDeallocPt(Pt);
n = (double)m_cloudSize;
#endif // ANN
}else
{ // Point to mesh
assert(m_cloud!=0 && m_modelMesh!=0);
RMS.set_size((unsigned int)m_modelMesh->getTriangles().size());
RMS.fill(0.0);
nbPoints.set_size(RMS.size());
nbPoints.fill(0.0);
unsigned int i;
arlCore::Point::sptr point = arlCore::Point::New(3);
for( i=0 ; i<m_cloud->size() ; ++i )
if(m_cloud->get(i))
if(!m_justVisible || m_cloud->get(i)->isVisible())
{
InvM.trf(m_cloud->get(i), point);
const double SquaredDist = m_modelMesh->computeDistance2(point, noTriangle);
if(SquaredDist>0.0)
{
RMS[noTriangle] += SquaredDist;
if(fx.size()>i) fx[i] = SquaredDist;
nbPoints[noTriangle] += 1;
}
}
}
assert(nbPoints.size()==RMS.size());
if(RMS.size()==1)
{
if(nbPoints[0]>0) return sqrt(RMS[i]/nbPoints[i]);
else return -1.0;
}
for( i=0 ; i<RMS.size() ; ++i )
if(nbPoints[i]>0)
{
result += sqrt(RMS[i]/nbPoints[i]);
n += 1.0;
}
if(n>0) return result/n;
else return -1;
}
bool rgrsn_ldp::viterbi(const vnl_matrix<double> & prob_map, const vnl_vector<double> & transition,
vcl_vector<int> & optimal_bins)
{
const int N = prob_map.rows();
const int nBin = prob_map.cols();
const int nNeighborBin = transition.size()/2;
const double epsilon = 0.01;
// dynamic programming
vnl_matrix<double> log_accumulatedProbMap = vnl_matrix<double>(N, nBin);
log_accumulatedProbMap.fill(0.0);
vnl_matrix<int> lookbackTable = vnl_matrix<int>(N, nBin);
lookbackTable.fill(0);
// copy first row
for (int c = 0; c<prob_map.cols(); c++) {
log_accumulatedProbMap[0][c] = log(prob_map[0][c] + epsilon);
lookbackTable[0][c] = c;
}
vnl_vector<double> log_transition = vnl_vector<double>(transition.size(), 0);
for (int i = 0; i<transition.size(); i++) {
log_transition[i] = log(transition[i] + epsilon);
}
for (int r = 1; r <N; r++) {
for (int c = 0; c<prob_map.cols(); c++) {
// lookup all possible place in the window
double max_val = vcl_numeric_limits<int>::min();
int max_index = -1;
for (int w = -nNeighborBin; w <= nNeighborBin; w++) {
if (c + w < 0 || c + w >= prob_map.cols()) {
continue;
}
assert(w + nNeighborBin >= 0 && w + nNeighborBin < transition.size());
double val = log_accumulatedProbMap[r-1][c+w] + log_transition[w + nNeighborBin];
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);
log_accumulatedProbMap[r][c] = max_val + log(prob_map[r][c] + epsilon);
lookbackTable[r][c] = max_index;
}
}
// lookback the table
double max_prob = vcl_numeric_limits<int>::min();
int max_prob_index = -1;
for (int c = 0; c<log_accumulatedProbMap.cols(); c++) {
if (log_accumulatedProbMap[N-1][c] > max_prob) {
max_prob = log_accumulatedProbMap[N-1][c];
max_prob_index = c;
}
}
// back track
optimal_bins.push_back(max_prob_index);
for (int r = N-1; r > 0; r--) {
int bin = lookbackTable[r][optimal_bins.back()];
optimal_bins.push_back(bin);
}
assert(optimal_bins.size() == N);
vcl_reverse(optimal_bins.begin(), optimal_bins.end());
return true;
}
