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
    }
  }
}

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;
}

/// \brief		Nonlinear process model for needle steering
void UnscentedKalmanFilter::f(vnl_vector<double> x1, vnl_vector<double> u, 
	vnl_vector<double> &x2)
{
	// initialize the output to 0
	x2.fill(0.0);
	// isolate the position and orientation components of the input vector
	vnl_vector<double> p = x1.extract(3,0);
	vnl_vector<double> r = x1.extract(3,3);
	// change rotation vector representation to quaternion
	vnl_vector<double> r_norm = r;
	r_norm.normalize();
	vnl_quaternion<double> q(r_norm,r.magnitude());
	
	// isolate specific input variables
	double d_th = u[0];
	double ro = u[1];
	double l = u[2];

	// define x,z axes as vectors
	vnl_matrix<double> I(3,3); I.set_identity();
	vnl_vector<double> x_axis = I.get_column(0);
	vnl_vector<double> z_axis = I.get_column(2);

	// Update position
	
	// define rotation matrix for d_th about z_axis
	vnl_matrix<double> Rz_dth = vnl_rotation_matrix( (d_th*z_axis) );
	
	// define circular trajectory in y-z plane
	vnl_vector<double> circ(3,0.0);
	circ[1] = ro*(1-cos(l/ro));
	circ[2] = ro*sin(l/ro);

	// define delta position vector in current frame
	vnl_vector<double> d_p = Rz_dth*circ;

	// Transform delta vector into world frame using quaternion rotation
	vnl_vector<double> d_p_world = q.rotate(d_p);

	// add rotated vector to original position
	vnl_vector<double> p2 = d_p_world + p;

	// Update orientation

	// form quaternions for x-axis and z-axis rotations
	vnl_quaternion<double> q_b(z_axis,d_th);
	vnl_quaternion<double> q_a(x_axis,-l/ro);
	// multiply original orientation quaternion
	vnl_quaternion<double> q2 = q*q_b*q_a;
	vnl_vector<double> r2 = q2.axis()*q2.angle();

	// Compose final output
	for( int i = 0; i < 3; i++)
	{
		x2[i] = p2[i];
		x2[i+3] = r2[i];
	}
}

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;
}

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::fieldCalibration( PointList::csptr real, PointList::csptr distorded, unsigned int degree, vnl_vector<double> &parameters, double &RMS )
{
    if(degree<1) return false;
    Polynomial_cost_function pcf( real, distorded, degree );
    vnl_powell op(&pcf);
    parameters.set_size(pcf.getNbParameters());
    parameters.fill(0.0);
    op.minimize(parameters);
    RMS = op.get_end_error();
    return true;
}

// 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));
  }
}

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;
}

/// \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 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;
}

//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;
	}

bool arlCore::FieldCorrector::setParameters( const vnl_vector<double> &parameters )
{
    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;
}

void TpsRegistration::SetParam(vnl_vector<double>& x0) {
  int k = 0;
  x0.set_size(func_->get_number_of_unknowns());
  x0.fill(0);
  if (!func_->fix_affine_) { // x0 includes affine
    for (int i = 0; i < param_affine_.rows(); ++i) {
      for (int j = 0; j < d_; ++j, ++k) {
        x0[k] = param_affine_(i, j);
      }
    }
  }
  for (int i = 0; i < param_tps_.rows(); ++i) {
    for (int j = 0; j < d_; ++j, ++k) {
      x0[k] = param_tps_(i, j);
    }
  }
}

	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;
	}

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;
}

	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;
	}

// 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);
}