Ejemplo n.º 1
0
bool ImplicitSurface::intersect(const Ray& ray, IntersectionInfo& info) {
    if (boundingGeom != NULL && !boundingGeom->intersect(ray, info)) {
        return false;
    }
    double lastValue, currentValue;
    double step, minStep = 1e-4;
    double lastDistance, currentDistance;
    lastValue = currentValue = f(ray.start.x, ray.start.y, ray.start.z);
    if (maxGradient.length() > 1e-6) {
        minStep = fabs(currentValue / (maxGradient.x * ray.dir.x));
        minStep = std::min(minStep, fabs(currentValue / (maxGradient.y * ray.dir.y)));
        minStep = std::min(minStep, fabs(currentValue / (maxGradient.z * ray.dir.z)));
    }
    lastDistance = currentDistance = 0.0;
    Vector point = ray.start, gradient;
    while(currentDistance < 1e99 && fabs(currentValue) > 1e-7 && std::signbit(currentValue) == std::signbit(lastValue)) {
        gradient = computeGradient(point);
        step = fabs(currentValue / (gradient.x * ray.dir.x));
        step = std::min(step, fabs(currentValue / (gradient.y * ray.dir.y)));
        step = std::min(step, fabs(currentValue / (gradient.z * ray.dir.z)));
        step = std::max(step, minStep);
        lastDistance = currentDistance;
        lastValue = currentValue;
        currentDistance += step;
        point = ray.start + ray.dir * currentDistance;
        currentValue = f(point.x, point.y, point.z);
    }
    if (currentDistance > 1e99) { return false; }
    double middleValue, middleDistance;
    while (fabs(currentValue) > 1e-7 || std::signbit(currentValue)) {
        middleDistance = (lastDistance + currentDistance) / 2;
        point = ray.start + ray.dir * middleDistance;
        middleValue = f(point.x, point.y, point.z);
        if (std::signbit(middleValue) != std::signbit(lastValue)) {
            currentValue = middleValue;
            currentDistance = middleDistance;
        }
        else {
            lastDistance = currentDistance;
            lastValue = currentValue;
            currentValue = middleValue;
            currentDistance = middleDistance;
        }
    }
    info.distance = currentDistance;
	info.ip = ray.start + ray.dir * currentDistance;
	info.normal = computeGradient(info.ip);
	info.normal.normalize();
	info.u = info.v = 0;
	info.geom = this;
    return true;
}
Ejemplo n.º 2
0
void computeAffineMap(
    const Mat1f &scale_map,
    const Mat1f &depth_map,
    float sw,
    float min_px_scale,
    Mat3f& affine_map )
{
  affine_map.create( depth_map.rows, depth_map.cols );

  const float nan = std::numeric_limits<float>::quiet_NaN();
  for ( int y = 0; y < depth_map.rows; y++ )
  {
    for ( int x = 0; x < depth_map.cols; ++x )
    {
      Vec2f grad;
      const float sp = getScale(scale_map[y][x], sw);
      if ( isnan(sp) || sp < min_px_scale ||
           !computeGradient( depth_map, x, y, sp, grad ) )
      {
        affine_map[y][x][0] = nan;
        affine_map[y][x][1] = nan;
        affine_map[y][x][2] = nan;
      }
      else
      {
        getAffParams( sw, grad, affine_map[y][x] );
      }
    }
  }
}
Ejemplo n.º 3
0
void computeAffineMapFixed(
    const Mat1f &depth_map,
    float sp,
    float f,
    Mat3f& affine_map )
{
  affine_map.create( depth_map.rows, depth_map.cols );

  const float sp_div_f = sp / f;

  for ( int y = 0; y < depth_map.rows; y++ )
  {
    for ( int x = 0; x < depth_map.cols; ++x )
    {
      Vec2f grad;
      if ( isnan(depth_map[y][x]) ||
           !computeGradient( depth_map, x, y, sp, grad ) )
      {
        affine_map[y][x][0] = nan;
        affine_map[y][x][1] = nan;
        affine_map[y][x][2] = nan;
      }
      else
      {
        const float sw = sp_div_f * depth_map[y][x];
        getAffParams( sw, grad, affine_map[y][x] );
      }
    }
  }
}
Ejemplo n.º 4
0
LieAlgebra MinEnFilter::integrateGImplicit(const LieAlgebra& grad, const MatrixXd& Gk, const MatrixXd& Disps_inhom ) {
	
	LieAlgebra K = LieAlgebra(order);
	LieAlgebra U;
		
	for (size_t i=0; i < NUM_FIXEDPOINT_ITERATIONR; i++) {
		U = computeGradient(currG * ((0.5 * K).exp_g()), Gk, Disps_inhom);
		K = delta * dynamicsE(U, currG);	
	}
	return K;
}
jboolean Java_androidx_media_filterpacks_image_SobelFilter_sobelOperator(
    JNIEnv* env, jclass clazz, jint width, jint height, jobject imageBuffer,
    jobject magBuffer, jobject dirBuffer) {

  if (imageBuffer == 0) {
    return JNI_FALSE;
  }
  unsigned char* srcPtr = static_cast<unsigned char*>(env->GetDirectBufferAddress(imageBuffer));
  unsigned char* magPtr = (magBuffer == 0) ?
      0 : static_cast<unsigned char*>(env->GetDirectBufferAddress(magBuffer));
  unsigned char* dirPtr = (dirBuffer == 0) ?
      0 : static_cast<unsigned char*>(env->GetDirectBufferAddress(dirBuffer));

  int numPixels = width * height;
  // TODO: avoid creating and deleting these buffers within this native function.
  short* gxPtr = new short[3 * numPixels];
  short* gyPtr = new short[3 * numPixels];
  computeGradient(srcPtr, width, height, gxPtr, gyPtr);

  unsigned char* mag = magPtr;
  unsigned char* dir = dirPtr;
  for (int i = 0; i < numPixels; ++i) {
    for (int c = 0; c < 3; c++) {
      int gx = static_cast<int>(*(gxPtr + 3 * i + c) / 8 + 127.5);
      int gy = static_cast<int>(*(gyPtr + 3 * i + c) / 8 + 127.5);

      // emulate arithmetic in GPU.
      gx = 2 * gx - 255;
      gy = 2 * gy - 255;
      if (magPtr != 0) {
        double value = sqrt(gx * gx + gy * gy);
        *(magPtr + 4 * i + c) = static_cast<unsigned char>(value);
      }
      if (dirPtr != 0) {
        *(dirPtr + 4 * i + c) = static_cast<unsigned char>(
            (atan(static_cast<double>(gy)/static_cast<double>(gx)) + 3.14) / 6.28);
      }
    }
    //setting alpha change to 1.0 (255)
    if (magPtr != 0) {
      *(magPtr + 4 * i + 3) = 255;
    }
    if (dirPtr != 0) {
      *(dirPtr + 4 * i + 3) = 255;
    }
  }

  delete[] gxPtr;
  delete[] gyPtr;

  return JNI_TRUE;
}
Ejemplo n.º 6
0
// Run L-BFGS-B Solver on f(x) until completion 
SolverExitStatus Solver::runSolver() {
  SolverExitStatus status = success;  // The return value.

  // This initial call sets up the structures for L-BFGS.
  callLBFGS("START");

  // Repeat until we've reached the maximum number of iterations.
  while (true) {

    // Do something according to the "task" from the previous call to
    // L-BFGS.
    if (strIsEqualToCStr(task,"FG")) {

      // Evaluate the objective function and the gradient of the
      // objective at the current point x.
      *f = computeObjective(x);
      computeGradient(x);
   //   printf("g = %f\n", g[0]);
    } 
    
    else if (strIsEqualToCStr(task,"NEW_X")) {

       // Go to the next iteration and call the iterative callback
       // routine.
       iter++;
       //printf("iter %d\n", iter);

       // If we've reached the maximum number of iterations, terminate
       // the optimization.
       if (iter == maxiter) {
          callLBFGS("STOP");
          break;
       }
    } 
    
    else if (strIsEqualToCStr(task,"CONV"))
       break;
    else if (strIsEqualToCStr(task,"ABNO")) {
       status = abnormalTermination;
       break;
    } 
    else if (strIsEqualToCStr(task,"ERROR")) {
       status = errorOnInput;
       break;
    }

    // Call L-BFGS again.
    callLBFGS();
  }

  return status;
}
Ejemplo n.º 7
0
Matrix4d MinEnFilter::iterate(MatrixXd& XY, VectorXd& Depth, MatrixXd& Flow) {
	cout << "Iterate: " << iteration << endl;
	iteration ++;
	
	LieAlgebra grad, KE;
	MatrixXd Hessian, KP;
	
	
	// get and check dimensions
	nPoints = XY.rows();
	
	if (nPoints!= Depth.rows()) {
		cerr << "Dimensions of XY do not coincide with Dimensions of Depth." << endl;
	}
	if (nPoints!= Flow.rows()) {
		cerr << "Dimensions of XY do not coincide with Dimensions of Flow." << endl;
	}
	
	MatrixXd XYZ_inhom  = inhomCoordinates(XY, false);		// inhomogenous Coordinatestes
	MatrixXd Flow_inhom = inhomCoordinates(Flow, true);		// inhomogenous (relative) flow
	MatrixXd Disps_inhom = XYZ_inhom + Flow_inhom;			// inhomogenous Disparities
	MatrixXd temp(nPoints, 4);
	
	temp << XYZ_inhom, Depth.cwiseInverse();
	MatrixXd Gk = (Depth*MatrixXd::Ones(1,4)).cwiseProduct(temp);

	
	// compute Gradient of Hamiltonian
	grad = computeGradient(currG , Gk, Disps_inhom);
	// compute Hessian of Hamiltonian
	Hessian = computeHessian(Gk, Disps_inhom);

	
	// Numerical integration of state and second order information
	for (size_t i = 0; i < numSteps; i++ ){
		KE = integrateGImplicit(grad, Gk, Disps_inhom);
		currG = currG * KE.exp_g();
		KP = integratePExplicit(grad ,Hessian);
		currP = currP + delta * KP;
	}
	
	return currG.E;
}
void LogisticRegressionByNonlinearConjugateGradient::train() {

	double fval = 0;

	/* Minimize the cross-entropy error function defined by
	 * E (W) = -ln p (T|w1,w2,...,wK) / nSample
	 * Gradient: G = X * (V - Y) / nSample
	 */
	// W = repmat(zeros(nFea, 1), new int[]{1, K});
	DenseMatrix& W = *new DenseMatrix(nFeature + 1, nClass, 0);
	Matrix& A = *new DenseMatrix(nExample, nClass, 0);
	// A = X.transpose().multiply(W);
	computeActivation(A, *X, W);
	Matrix& V = sigmoid(A);
	Matrix& G = W.copy();

	// G = X.multiply(V.subtract(Y)).scalarMultiply(1.0 / nSample);
	Matrix&  VMinusY = *new DenseMatrix(nExample, nClass, 0);
	minus(VMinusY, V, *Y);
	// mtimes(G, XT, VMinusY);
	computeGradient(G, *X, VMinusY);
	timesAssign(G, 1.0 / nExample);


	// fval = -sum(sum(times(Y, log(plus(V, eps))))).getEntry(0, 0) / nSample;
	Matrix& YLogV = *new DenseMatrix(nExample, nClass);
	Matrix& VPlusEps = *new DenseMatrix(nExample, nClass);
	Matrix& LogV = *new DenseMatrix(nExample, nClass);
	plus(VPlusEps, V, eps);
	log(LogV, VPlusEps);
	times(YLogV, *Y, LogV);
	fval = -sum(sum(YLogV)) / nExample;

	bool* flags = new bool[2];
	while (true) {
		NonlinearConjugateGradient::run(G, fval, epsilon, W, flags);
		/*disp("W:");
		disp(W);*/
		if (flags[0])
			break;
		// A = X.transpose().multiply(W);
		computeActivation(A, *X, W);
		/*disp("A:");
		disp(A);*/
		// V = sigmoid(A);
		sigmoid(V, A);
		// fval = -sum(sum(times(Y, log(plus(V, eps))))).getEntry(0, 0) / nSample;
		plus(VPlusEps, V, eps);
		/*disp("V:");
		disp(V);*/
		log(LogV, VPlusEps);
		times(YLogV, *Y, LogV);
		/*disp("YLogV:");
		disp(YLogV);*/
		fval = -sum(sum(YLogV)) / nExample;
		// fprintf("fval: %.4f\n", fval);
		if (flags[1]) {
			// G = rdivide(X.multiply(V.subtract(Y)), nSample);
			minus(VMinusY, V, *Y);
			// mtimes(G, XT, VMinusY);
			computeGradient(G, *X, VMinusY);
			timesAssign(G, 1.0 / nExample);
		}
	}
	double** WData = W.getData();
	double** thisWData = new double*[nFeature];
	for (int feaIdx = 0; feaIdx < nFeature; feaIdx++) {
		thisWData[feaIdx] = WData[feaIdx];
	}
	this->W = new DenseMatrix(thisWData, nFeature, nClass);
	b = WData[nFeature];

	// delete &W;
	delete &A;
	delete &V;
	delete &G;
	delete &VMinusY;
	delete &YLogV;
	delete &VPlusEps;
	delete &LogV;
	delete[] flags;

}
Ejemplo n.º 9
0
CvPoint2D32f getPupilCenter(Mat &eye_box){
	//find x and y gradients
	Mat gradientX = computeGradient(eye_box);
	Mat gradientY = computeGradient(eye_box.t()).t();

	//normalize and threshold the gradient
	Mat mags = matrixMagnitude(gradientX, gradientY);

	//create a blurred and inverted image for weighting
	Mat weight;
	bitwise_not(eye_box, weight);
	blur(weight, weight, Size(2,2));

	//weight the magnitudes, convert to 8-bit for thresholding
	weight.convertTo(weight, CV_32F);
	mags = mags.mul(weight);
	normalize(mags, mags, 0, 1, NORM_MINMAX, CV_32F);
	mags.convertTo(mags, CV_8UC1, 255);

	//threshold using Otsu's method
	threshold(mags, mags, 0, 255, THRESH_BINARY | THRESH_OTSU);

	//convert to CV_32S and filter gradients
	mags.convertTo(mags, CV_32S);
	gradientY = gradientY.mul(mags);
	gradientX = gradientX.mul(mags);

	//resize arrays to same size
	resize(gradientX, gradientX, Size(EYE_FRAME_SIZE, EYE_FRAME_SIZE), 0, 0, INTER_NEAREST);
	resize(gradientY, gradientY, Size(EYE_FRAME_SIZE, EYE_FRAME_SIZE), 0, 0, INTER_NEAREST);
	resize(weight, weight, Size(EYE_FRAME_SIZE, EYE_FRAME_SIZE), 0, 0, INTER_NEAREST);

	//imshow("gradY", gradientY * 255);
	//imshow("weight", weight / 255);

	//run the algorithm:
	//	for each possible gradient location
	//	Note: these loops are reversed from the way the paper does them
	//	it evaluates every possible center for each gradient location instead of
	//	every possible gradient location for every center.
	Mat out = Mat::zeros(weight.rows,weight.cols, CV_32F);
	float max_val = 0;
	//for all pixels in the image
	for (int y = 0; y < EYE_FRAME_SIZE; ++y) {
		const int *grad_x = gradientX.ptr<int>(y), *grad_y = gradientY.ptr<int>(y);
		for (int x = 0; x < EYE_FRAME_SIZE; ++x) {
			int gX = grad_x[x], gY = grad_y[x];
			if (gX == 0 && gY == 0) {
				continue;
			}
			//for all possible centers
			for (int cy = 0; cy < EYE_FRAME_SIZE; ++cy) {
				float *Or = out.ptr<float>(cy);
				const float *Wr = weight.ptr<float>(cy);
				for (int cx = 0; cx < EYE_FRAME_SIZE; ++cx) {
					//ignore center of box
					if (x == cx && y == cy) {
						continue;
					}
					//create a vector from the possible center to the gradient origin
					int dx = x - cx;
					int dy = y - cy;

					//compute dot product using lookup table
					float dotProduct;
					if(dx > 0 && dy > 0){
						dotProduct = dpX[dx+EYE_FRAME_SIZE*dy]*gX + dpY[dx+EYE_FRAME_SIZE*dy]*gY;
					}else if(dx > 0){
						dotProduct = dpX[dx-EYE_FRAME_SIZE*dy]*gX - dpY[dx-EYE_FRAME_SIZE*dy]*gY;
					}else if(dy > 0){
						dotProduct = -dpX[-dx+EYE_FRAME_SIZE*dy]*gX - dpY[-dx+EYE_FRAME_SIZE*dy]*gY;
					}else{
						dotProduct = -dpX[-dx-EYE_FRAME_SIZE*dy]*gX - dpY[-dx-EYE_FRAME_SIZE*dy]*gY;
					}

					//ignore negative dot products as they point away from eye
					if(dotProduct <= 0.0){
						continue;
					}

					//square and multiply by the weight
					Or[cx] += dotProduct * dotProduct * Wr[cx];

					//compare with max
					if(Or[cx] > max_val){
						max_val = Or[cx];
					}
				}
			}
		}
	}

	//resize for debugging
	resize(out, out, Size(500,500), 0, 0, INTER_NEAREST);

	out = 255 * out / max_val;
	//imshow("calc", out / 255);

	//histogram setup
	Mat hist;
	int histSize = 256;
	float range[] = { 0, 256 } ;
	const float* histRange = { range };
	//calculate the histogram
	calcHist(&out,1, 0, Mat(), hist, 1, &histSize, &histRange,
		true,	//uniform
		true	//accumulate
	);

	//get cutoff for top 10 pixels
	float top_end_sum = 0;
	int top_end = 0.92 * 255;
	for (int i = 255; i > 0; i--) {
		top_end_sum += hist.at<float>(i);
		if(top_end_sum > 3000){
			top_end = i;
			break;
		}
	}

	//draw image for debugging
	Mat histImage(400, 512, CV_8UC3, Scalar(0,0,0));
	int bin_w = cvRound( (double) 512/histSize );
	normalize(hist, hist, 0, histImage.rows, NORM_MINMAX, -1, Mat());
	/// Draw for each channel
	for( int i = 1; i < histSize; i++)
	{
		line(histImage, Point(bin_w*(i), 400 - cvRound(hist.at<float>(i))),
						Point(bin_w*(i), 400),
						Scalar(i, i, i), 2, 8, 0);
	}
	//imshow("hist", histImage);

	//threshold to get just the pupil
	//printf("top_end: %d\n", top_end);
	threshold(out, out, top_end, 255, THRESH_TOZERO);

	//calc center of mass
	float sum = 0;
	float sum_x = 0;
	float sum_y = 0;
	for (int y = 0; y < out.rows; ++y)
	{
		float* row = out.ptr<float>(y);
		for (int x = 0; x < out.cols; ++x)
		{
			float val = row[x]*row[x];
			if(val > 0){
				sum += val;
				sum_x += val*x;
				sum_y += val*y;
			}
		}
	}
	Size eye_box_size = eye_box.size();
	Size out_size = out.size();
	//cout << "Size1: "+to_string(eye_box_size.width)+","+to_string(eye_box_size.height)+"\n";
	//cout << "Size2: "+to_string(out_size.width)+","+to_string(out_size.height)+"\n";
	float x_scale = (float) eye_box_size.width / out_size.width;
	float y_scale = (float) eye_box_size.height / out_size.height;
	CvPoint2D32f max = cvPoint2D32f(x_scale*sum_x/sum, y_scale*sum_y/sum);
	//circle(out, max, 3, 0);
	//imshow("thresh", out / 255);
	return max;
}
double Gradient::computeGradient(dVector& vecGradient, Model* m, DataSet* X)
{
	//Check the size of vecGradient
	int nbFeatures = pFeatureGen->getNumberOfFeatures();
	double ans = 0.0;
	int TID = 0;
	if(vecGradient.getLength() != nbFeatures)
		vecGradient.create(nbFeatures);
	else
		vecGradient.set(0);
	// Initialize the buffers (vecFeaturesMP) for each thread
#ifdef _OPENMP
	setMaxNumberThreads(omp_get_max_threads());
	pInfEngine->setMaxNumberThreads(omp_get_max_threads());
	pFeatureGen->setMaxNumberThreads(omp_get_max_threads());
#endif
	for(int t=0;t<nbThreadsMP;t++)
	{
		if(localGrads[t].getLength() != nbFeatures)
			localGrads[t].resize(1,nbFeatures,0);
		else
			localGrads[t].set(0);
	}

////////////////////////////////////////////////////////////
// Start of parallel Region

	// Some weird stuff in gcc 4.1, with openmp 2.5 support
#if ((_OPENMP == 200505) && __GNUG__)
#pragma omp parallel \
	shared(X, m, ans, nbFeatures, std::cout)	\
	private(TID) \
	default(none)
#else
#pragma omp parallel \
	shared(vecGradient, X, m, ans, nbFeatures, std::cout)	\
	private(TID) \
	default(none)
#endif
	{
#ifdef _OPENMP 
		TID = omp_get_thread_num();
#endif
		// Create a temporary gradient
		double localSum = 0;

#ifdef WITH_SEQUENCE_WEIGHTS
		dVector tmpVecGradient(nbFeatures);
#endif

#pragma omp for
		// we can use unsigned if we have openmp 3.0 support (_OPENMP>=200805).
#ifdef _OPENMP 
    #if _OPENMP >= 200805
		for(unsigned int i = 0; i< X->size(); i++){
    #else
	    for(int i = 0; i< X->size(); i++){
    #endif
#else
		for(unsigned int i = 0; i< X->size(); i++){
#endif
			if (m->getDebugLevel() >=2){
#pragma omp critical(output)
				std::cout << "Thread "<<TID<<" computes gradient for sequence " 
						  << i <<" out of " << (int)X->size() 
						  << " (Size: " <<  X->at(i)->length() << ")" << std::endl;
			}
			DataSequence* x = X->at(i);
#ifdef WITH_SEQUENCE_WEIGHTS
			tmpVecGradient.set(0);
			localSum += computeGradient(tmpVecGradient, m, x) * x->getWeightSequence();
			if(x->getWeightSequence() != 1.0)
				tmpVecGradient.multiply(x->getWeightSequence());
			localGrads[TID].add(tmpVecGradient);
#else
			localSum += computeGradient(localGrads[TID], m, x);// * x->getWeightSequence();
#endif
		}
#pragma omp critical (reduce_sum)
		// We now put togheter the sums
		{
			if( m->getDebugLevel() >= 2){
				std::cout<<"Thread "<<TID<<" update sums"<<std::endl;
			}
			ans += localSum;
			vecGradient.add(localGrads[TID]);
		}
	} 
	
// End of parallel Region
////////////////////////////////////////////////////////////

	// because we are minimizing -LogP
	vecGradient.negate();

	// Add the regularization term
	double sigmaL2Square = m->getRegL2Sigma()*m->getRegL2Sigma();
	if(sigmaL2Square != 0.0f) {
		if (m->getDebugLevel() >= 2){
			std::cout << "Adding L2 norm gradient\n";
		}
		for(int f = 0; f < nbFeatures; f++) {
			vecGradient[f] += (*m->getWeights())[f]/sigmaL2Square;
		}
		double weightNorm = m->getWeights()->l2Norm(false);
		ans += weightNorm / (2.0*m->getRegL2Sigma()*m->getRegL2Sigma());	
	}
	return ans;
}
Ejemplo n.º 11
0
void MlMaximumEntropyModel::learnLMBFGS(MlTrainingContainer* params)
{
	params->initialize();

	const MlDataSet* trainingDataSet = params->getTrainingDataSet();
	const MlDataSet* testDataSet     = params->getTestDataSet();
	const vector<size_t>& trainingIdxs = params->getTrainingIdxs();
	const vector<size_t>& testIdxs	   = params->getTestIdxs();
	const size_t numSamples = trainingIdxs.size();
	const bool performTest = (testDataSet && testIdxs.size()>0);

	if (trainingDataSet->getNumClasess() < 2)
		error("learnLMBFGS accepts only datasets with 2 or more classes, yor data has ",
			trainingDataSet->getNumClasess());

	const double  lambda  = params->getLambda();
	const size_t memorySize = params->getLmBfgsMemorySize(); 
	const size_t reportFrequency = params->getVerboseLevel();
	const double perplexityDelta  = params->getPerplexityDelta();
	const size_t numClasses  = trainingDataSet->getNumClasess();
	const size_t numFeatures = trainingDataSet->getNumBasicFeatures(); // F
	const size_t numTraining = trainingIdxs.size();    // N

	const size_t numRestarts=3;

	// data structures used for training
	vector< vector<double> >& w = weights_; // class X features
	vector< vector<double> > wOld(numClasses, vector<double>(numFeatures,0.0)); // class X features
	vector< vector<double> > wtx(numClasses, vector<double>(numTraining, 0.0));  // class X samples
	vector< vector<double> > qtx(numClasses, vector<double>(numTraining, 0.0));  // class X samples
	vector< vector<double> > q(numClasses, vector<double>(numFeatures,0.0));       // class X features
	vector< vector<double> > g(numClasses, vector<double>(numFeatures,0.0));       // class X features
	vector< vector<double> > gOld(numClasses, vector<double>(numFeatures,0.0));  // class X features

	vector< vector<float> > trainingProbs(numClasses, vector<float>(numTraining));
	vector< vector<float> > testProbs(numClasses, vector<float>(numTraining));
	vector< vector<double> > bestW(numClasses, vector<double>(numFeatures));

	// initialize weights
	if (params->getInputPath().length() > 1)
	{
		const string modelFile = params->getInputPath() + "_scr.txt";
		if (readModel(modelFile.c_str()))
			params->setIndClearWeights(false);
	}

	if (params->getIndClearWeights())
		weights_.clear();

	weights_.resize(numClasses, vector<double>(numFeatures,0.0));

	double previousPerplexity = MAX_FLOAT;
	float  bestTestError=1.0;
	size_t bestTestRound=0;
	float  bestTrainingError=1.0;
	size_t bestTrainingRound=0;

	bool terminateTraining = false;
	size_t totalRounds=0;
	size_t megaRound=0;
	for ( megaRound=0; megaRound<numRestarts; megaRound++)
	{
		// first round
		computeGradient(trainingDataSet, trainingIdxs, w, wtx, lambda, g);

		const double gtg = computeDotProduct(g,g);
		const double denominator = 1.0 / sqrt(gtg);
		for (size_t c=0; c<numClasses; c++)
			for (size_t i=0; i<numFeatures; i++)
				q[c][i]=g[c][i]*denominator;

		// qtx <- qTx
		for (size_t c=0; c<numClasses; c++)
			for (size_t i=0; i<numSamples; i++)
			{
				const MlSample& sample = trainingDataSet->getSample(trainingIdxs[i]);
				qtx[c][i]=computeDotProduct(q[c],sample.pairs);
			}

		// eta <- lineSearch(...)
		double eta = lineSearch(trainingDataSet, trainingIdxs, w, wtx, qtx, g, q, lambda);
		//cout << "eta = " << eta << endl;

		// update wtx <- wtx + eta*qtx
		for (size_t c=0; c<numClasses; c++)
			for (size_t i=0; i<wtx[c].size(); i++)
				wtx[c][i]+=eta*qtx[c][i];

		// update wOld<- w ; w <- w + eta *q ; gOld<-g
		for (size_t c=0; c<numClasses; c++)
		{
			memcpy(&wOld[c][0],&w[c][0],sizeof(double)*w[c].size());
			memcpy(&gOld[c][0],&g[c][0],sizeof(double)*g[c].size());
			for (size_t i=0; i<numFeatures; i++)
				w[c][i]+= eta*q[c][i];
		}


		// initialize memory
		vector< vector< vector<double> > > memoryU(memorySize, vector< vector<double> >(numClasses));
		vector< vector< vector<double> > > memoryD(memorySize, vector< vector<double> >(numClasses));
		vector< double > memoryAlpha(memorySize);
		size_t nextMemPosition=0;
		size_t numMemPushes=0;
		
		// iterate until convergence
		size_t round=1;
		while (round<10000)
		{
			// compute errors and report round results
			{
				double trainingLogLikelihood=0.0, testLogLikelihood=0.0;
				const double trainingError = calcErrorRateWithLogLikelihood(trainingDataSet, trainingIdxs,
																		false, &trainingLogLikelihood);
				double testError=1.0;
				if (performTest)
					testError = calcErrorRateWithLogLikelihood(testDataSet, testIdxs, false, &testLogLikelihood);

				if (reportFrequency>0 && round % reportFrequency == 0)
				{
					cout << round << "\t" << scientific << setprecision(5) << trainingLogLikelihood << "\t" << fixed << setprecision(5) << trainingError;
					if (performTest)
						cout <<"\t" << scientific << testLogLikelihood << "\t" << fixed << setprecision(5)<< testError;
					cout << endl;
				}
				
				if (performTest)
				{
					if (testError<=bestTestError)
					{
						bestTestRound=round;
						bestTestError=testError;
						for (size_t c=0; c<numClasses; c++)
							memcpy(&bestW[c][0],&w[c][0],numFeatures*sizeof(double)); // copy weights
					}
				}
				
				if (trainingError<=bestTrainingError)
				{
					bestTrainingRound=round;
					bestTrainingError=trainingError;
					if (! performTest)
					{
						for (size_t c=0; c<numClasses; c++)
							memcpy(&bestW[c][0],&w[c][0],numFeatures*sizeof(double)); // copy weights
					}
				}		
			}

			// Train new round

			computeGradient(trainingDataSet, trainingIdxs, w, wtx, lambda, g);

			double alpha=0.0;
			double sigma=0.0;
			double utu=0.0;

			// write u=g'-g and d=w'-w onto memory, use them to compute alpha and sigma
			vector< vector<double> >& u = memoryU[nextMemPosition];
			vector< vector<double> >& d = memoryD[nextMemPosition];
			for (size_t c=0; c<numClasses; c++)
			{
				const size_t numFeatures = g[c].size();
				u[c].resize(numFeatures);
				d[c].resize(numFeatures);
				for (size_t i=0; i<numFeatures; i++)
				{
					const double gDiff = g[c][i]-gOld[c][i];
					const double wDiff = w[c][i]-wOld[c][i];
					u[c][i]=gDiff;
					d[c][i]=wDiff;
					alpha += gDiff*wDiff;
					utu += gDiff*gDiff;
				}
			}
			sigma = alpha / utu;
			memoryAlpha[nextMemPosition]=alpha;

			// update memory position
			nextMemPosition++;
			if (nextMemPosition == memorySize)
				nextMemPosition = 0;
			numMemPushes++;

			// q<-g
			for (size_t c=0; c<numClasses; c++)
				memcpy(&q[c][0],&g[c][0],g[c].size()*sizeof(double));
			
			// determine memory evaluation order 1..M (M is the newest)
			vector<size_t> memOrder;
			if (numMemPushes<=memorySize)
			{
				for (size_t i=0; i<numMemPushes; i++)
					memOrder.push_back(i);
			}
			else
			{
				for (size_t i=0; i<memorySize; i++)
					memOrder.push_back((i+nextMemPosition) % memorySize);
			}

			vector<double> beta(memOrder.size(),0.0);
			for (int i=memOrder.size()-1; i>=0; i--)
			{
				const size_t m = memOrder[static_cast<size_t>(i)];
				const double alpha = memoryAlpha[m];
				
				const vector< vector<double> >& dM = memoryD[m];
				double& betaM = beta[m];

				// compute beta[m] = (memory_d[m] dot g)/alpha[m]
				for (size_t c=0; c<dM.size(); c++)
					for (size_t i=0; i<dM[c].size(); i++)
						betaM += dM[c][i]*g[c][i];
				betaM/=alpha;
				
				// q <- q - beta[m]*memory_u[m]
				const vector< vector<double> >& uM = memoryU[m]; 
				for (size_t c=0; c<q.size(); c++)
					for (size_t i=0; i<q[c].size(); i++)
						q[c][i] -= betaM * uM[c][i];

			}

			// q <- sigma*q
			for (size_t c=0; c<q.size(); c++)
				for (size_t i=0; i<q[c].size(); i++)
					q[c][i]*=sigma;


			for (size_t i=0; i<memOrder.size(); i++)
			{
				const size_t m = memOrder[static_cast<size_t>(i)];
				const vector< vector<double> >& uM = memoryU[m];
				const vector< vector<double> >& dM = memoryD[m]; 
				const double betaM = beta[m];
				const double oneOverAlpha = 1.0 / memoryAlpha[m];
				double umq = computeDotProduct(uM,q);
				for (size_t c=0; c<numClasses; c++)
					for (size_t j=0; j<q[c].size(); j++)
					{
						const double dq = dM[c][j] * (betaM - umq*oneOverAlpha);
						umq += uM[c][j]*dq;
						q[c][j] += dq;
					}
		
			}

			// q<- -q
			for (size_t c=0; c<numClasses; c++)
				for (size_t i=0; i<q[c].size(); i++)
					q[c][i]=-q[c][i];
			
			// qtx = q*X
			for (size_t i=0; i<trainingIdxs.size(); i++)
			{
				const MlSample& sample = trainingDataSet->getSample(trainingIdxs[i]);
				for (size_t c=0; c<numClasses; c++)
					qtx[c][i]=computeDotProduct(q[c],sample.pairs);
			}
			

			bool needToRestart=false;
			eta = lineSearch(trainingDataSet, trainingIdxs, w, wtx, qtx, g, q, lambda);
			if (eta<= 0.0)
			{
				// restart ?
				needToRestart = true;
			}

			// update wOld<- w ; w <- w + eta *q ; gOld<- g
			for (size_t c=0; c<numClasses; c++)
			{
				memcpy(&wOld[c][0],&w[c][0],sizeof(double)*w[c].size());
				memcpy(&gOld[c][0],&g[c][0],sizeof(double)*g[c].size());
				for (size_t i=0; i<numFeatures; i++)
					w[c][i]+= eta*q[c][i];
			}

			for (size_t c=0; c<numClasses; c++)
				for (size_t i=0; i<numSamples; i++)
					wtx[c][i]+=eta*qtx[c][i];

			round++;
			totalRounds++;
			if (terminateTraining || needToRestart)
				break;
		}
		
		if (terminateTraining)
			break;
	}

	if (! params->getIndHadInternalError())
	{
		params->setIndNormalTermination(true);
	}
	else
		cout << "Warning: encountered mathemtical error while training!" << endl;

	weights_ = bestW;

	cout << "W=" << endl;
	printVector(weights_);
	cout << endl;

	cout << "Terminated after " << totalRounds << " rounds (" << megaRound << " restarts)" << endl;
	cout << "Best training error  " << fixed << setprecision(8) << bestTrainingError << " (round " << bestTrainingRound << ")" << endl;
	if (performTest)
	cout << "Best test error      "  << bestTestError     << " (round " << bestTestRound << ")" << endl;

	indWasInitialized_ = true;

	//this->calcErrorRateWithPerplexity(trainingDataSet, trainingIdxs, true, NULL);
}
Ejemplo n.º 12
0
Node *OutlineVectorizer::findOtherSide(Node *node)
{
	DataPixel *pix = node->m_pixel;

	TPoint dir = -computeGradient(pix);
	if (dir == TPoint(0, 0))
		return 0;
	TPoint d1(tsign(dir.x), 0), d2(0, tsign(dir.y));
	int num = abs(dir.y), den = abs(dir.x);
	if (num > den) {
		tswap(d1, d2);
		tswap(num, den);
	}
	TPoint pos = pix->m_pos;
	int i;
	for (i = 0;; i++) {
		TPoint q(pos.x + d1.x * i + d2.x * num * i / den, pos.y + d1.y * i + d2.y * num * i / den);
		DataPixel *nextPix = m_dataRaster->pixels(q.y) + q.x;
		if (nextPix->m_ink == false)
			break;
		pix = nextPix;
	}
	assert(pix);
	if (!pix->m_node) {
		const int wrap = m_dataRaster->getWrap();
		if (pix[-1].m_node)
			pix--;
		else if (pix[1].m_node)
			pix++;
		else if (pix[wrap].m_node)
			pix += wrap;
		else if (pix[-wrap].m_node)
			pix -= wrap;
		else {
			assert(0);
		}
	}
	if (!pix->m_node)
		return 0;
	Node *q = pix->m_node;
	while (q->m_pixel == 0 && q->m_other)
		q = q->m_other;
	assert(q && q->m_pixel == pix);

	for (i = 0; i < 5; i++) {
		if (!q->m_prev)
			break;
		q = q->m_prev;
	}

	Node *best = q;
	double bestDist2 = computeDistance2(q, node);
	for (i = 0; i < 10; i++) {
		q = q->m_next;
		if (!q)
			break;
		double dist2 = computeDistance2(q, node);
		if (dist2 < bestDist2) {
			bestDist2 = dist2;
			best = q;
		}
	}

	return best;
}
void vpTemplateTrackerMIInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
  if(!CompoInitialised)
    std::cout<<"Compositionnal tracking no initialised\nUse InitCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
  dW=0;

  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);

  int Nbpoint=0;


  double lambda=lambdaDep;
  double MI=0,MIprec=-1000;


  vpColVector p_avant_estimation;p_avant_estimation=p;
  MI_preEstimation=-getCost(I,p);
  NMI_preEstimation=-getNormalizedCost(I,p);

  //    std::cout << "MI avant: " << MI_preEstimation << std::endl;
  //    std::cout << "NMI avant: " << NMI_preEstimation << std::endl;

  initPosEvalRMS(p);

  vpColVector dpinv(nbParam);
  double alpha=2.;

  unsigned int iteration=0;

  //unsigned int bspline_ = (unsigned int) bspline;
  //unsigned int totParam = (bspline_ * bspline_)*(1+nbParam+nbParam*nbParam);

#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
  int Ncb_ = (int)Ncb;
  int nbParam_ = (int)nbParam;
  int sizePrtBis = Ncb_*Ncb_;
  int sizedPrtBis = Ncb_*Ncb_*nbParam_;
  int sized2PrtBis = Ncb_*Ncb_*nbParam_*nbParam_;
#endif

  vpMatrix Hnorm(nbParam,nbParam);

  do
  {
    Nbpoint=0;
    MIprec=MI;
    MI=0;

#ifndef USE_OPENMP_MI_INVCOMP
    zeroProbabilities();
#endif

    Warp->computeCoeff(p);

#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
#pragma omp parallel
#endif
    {
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
      double *Prtbis = new double[sizePrtBis];
      double *dPrtbis = new double[sizedPrtBis];
      double *d2Prtbis = new double[sized2PrtBis];

      unsigned int indd, indd2;
      indd = indd2 = 0;
      for(int i=0;i<Ncb_*Ncb_;i++){
        Prtbis[i]=0.0;
        Prt[i]=0.0;
        for(int j=0;j<nbParam_;j++){
          dPrtbis[indd]=0.0;
          dPrt[indd]=0.0;
          indd++;
          for(int k=0;k<nbParam_;k++){
            d2Prtbis[indd2]=0.0;
            d2Prt[indd2]=0.0;
            indd2++;
          }
        }
      }

#pragma omp barrier
#pragma omp for reduction(+:Nbpoint)
#endif
      for(int point=0;point<(int)templateSize;point++)
      {
        vpColVector x1(2),x2(2);
        double i2,j2;
        double IW;
        int cr,ct;
        double er,et;

        x1[0]=(double)ptTemplate[point].x;
        x1[1]=(double)ptTemplate[point].y;

        Warp->computeDenom(x1,p); // A modif pour parallelisation mais ne pose pas de pb avec warp utilises dans DECSA
        Warp->warpX(x1,x2,p);

        j2=x2[0];
        i2=x2[1];

        if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
        {

          //if(m_ptCurrentMask == NULL ||(m_ptCurrentMask->getWidth() == I.getWidth() && m_ptCurrentMask->getHeight() == I.getHeight() && (*m_ptCurrentMask)[(unsigned int)i2][(unsigned int)j2] > 128))
          {
            Nbpoint++;
            if(!blur)
              IW=(double)I.getValue(i2,j2);
            else
              IW=BI.getValue(i2,j2);

            ct=ptTemplateSupp[point].ct;
            et=ptTemplateSupp[point].et;
            double tmp = IW*(((double)Nc)-1.f)/255.f;
            cr=(int)tmp;
            er=tmp-(double)cr;

            if( (ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) && (ptTemplateSelect[point] || !useTemplateSelect) )
            {
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
              vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prtbis, dPrtbis, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
#else
              vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
#endif
            }
            else if (ptTemplateSelect[point] || !useTemplateSelect)
            {
              if(bspline==3){
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
                vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prtbis, dPrtbis, d2Prtbis, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#else
                vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#endif
                //                        {
                //                            // ################### AY : Optim
                //                            if(et>0.5){ct++;}
                //                            if(er>0.5){cr++;}
                //                            int index = (cr*Nc+ct)*totParam;
                //                            double *ptb = &PrtTout[index];
                //                            #endif
                //                            vpTemplateTrackerMIBSpline::PutTotPVBspline3(ptb, er, ptTemplateSupp[point].BtInit, size);
                //                            // ###################
                //                        }
              }
              else{
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
                vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prtbis, dPrtbis, d2Prtbis, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#else
                vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#endif
              }
            }
            else{
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
              vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prtbis, cr, er, ct, et, Ncb ,nbParam, bspline);
#else
              vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb,nbParam, bspline);
#endif
            }
          }

        }
      }

#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
#pragma omp critical
      {
        unsigned int indd, indd2;
        indd = indd2 = 0;
        for(int i=0;i<Ncb*Ncb;i++){
          Prt[i]+=Prtbis[i]/(double)Nbpoint;
          for(int j=0;j<nbParam_;j++){
            dPrt[indd]+=dPrtbis[indd]/(double)Nbpoint;
            indd++;
            for(int k=0;k<nbParam_;k++){
              d2Prt[indd2]+=d2Prtbis[indd2]/(double)Nbpoint;
              indd2++;
            }
          }
        }

        delete [] Prtbis;
        delete [] dPrtbis;
        delete [] d2Prtbis;
      }

#pragma omp barrier
#endif
    }

    if(Nbpoint==0)
    {
      diverge=true;
      MI=0;
      deletePosEvalRMS();
      throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));

    }
    else
    {
      //            computeProba(Nbpoint);

#ifndef USE_OPENMP_MI_INVCOMP
      unsigned int indd, indd2;
      indd = indd2 = 0;
      unsigned int Ncb_ = (unsigned int)Ncb;
      for(unsigned int i=0;i<Ncb_*Ncb_;i++){
        Prt[i]=Prt[i]/Nbpoint;
        for(unsigned int j=0;j<nbParam;j++){
          dPrt[indd]=dPrt[indd]/Nbpoint;
          indd++;
          for(unsigned int k=0;k<nbParam;k++){
            d2Prt[indd2]=d2Prt[indd2]/Nbpoint;
            indd2++;
          }
        }
      }
#endif

      computeMI(MI);

      if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE){
        computeHessienNormalized(Hnorm);
        computeHessien(H);
      }
      computeGradient();

      vpMatrix::computeHLM(H,lambda,HLM);

      try
      {
        switch(hessianComputation)
        {
        case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
          dp=gain*HLMdesireInverse*G;
          break;
        case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
          if(HLM.cond()>HLMdesire.cond())
            dp=gain*HLMdesireInverse*G;
          else
            dp=gain*0.2*HLM.inverseByLU()*G;
          break;
        default:
          dp=gain*0.2*HLM.inverseByLU()*G;
          break;
        }
      }
      catch(vpException &e)
      {
        //std::cerr<<"probleme inversion"<<std::endl;
        throw(e);
      }
    }

    switch(minimizationMethod)
    {
    case vpTemplateTrackerMIInverseCompositional::USE_LMA:
    {
      vpColVector dp_test_LMA(nbParam);
      vpColVector dpinv_test_LMA(nbParam);
      vpColVector p_test_LMA(nbParam);
      if(ApproxHessian==HESSIAN_NONSECOND)
        dp_test_LMA=-100000.1*dp;
      else
        dp_test_LMA=1.*dp;
      Warp->getParamInverse(dp_test_LMA,dpinv_test_LMA);
      Warp->pRondp(p,dpinv_test_LMA,p_test_LMA);

      MI=-getCost(I,p);
      double MI_LMA=-getCost(I,p_test_LMA);
      if(MI_LMA>MI)
      {
        dp=dp_test_LMA;
        lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
      }
      else
      {
        dp=0;
        lambda=(lambda*10.<1e6)?1e6:lambda*10.;
      }
    }
      break;
    case vpTemplateTrackerMIInverseCompositional::USE_GRADIENT:
      dp=-gain*0.3*G*20;
      break;

    case vpTemplateTrackerMIInverseCompositional::USE_QUASINEWTON:
    {
      double s_scal_y;
      if(iterationGlobale!=0)
      {
        vpColVector s_quasi=p-p_prec;
        vpColVector y_quasi=G-G_prec;
        s_scal_y=s_quasi.t()*y_quasi;
        //std::cout<<"mise a jour K"<<std::endl;
        /*if(s_scal_y!=0)//BFGS
                    KQuasiNewton=KQuasiNewton+0.01*(-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y);*/
        //if(s_scal_y!=0)//DFP
        if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())//DFP
        {
          KQuasiNewton=KQuasiNewton+0.0001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
          //std::cout<<"mise a jour K"<<std::endl;
        }
      }
      dp=gain*KQuasiNewton*G;
      //std::cout<<KQuasiNewton<<std::endl<<std::endl;
      p_prec=p;
      G_prec=G;
      //p-=1.01*dp;
    }
      break;

    default:
    {
      if(useBrent)
      {
        alpha=2.;
        computeOptimalBrentGain(I,p,-MI,dp,alpha);
        dp=alpha*dp;
      }
      if(ApproxHessian==HESSIAN_NONSECOND)
        dp=-1.*dp;

      break;
    }
    }

    Warp->getParamInverse(dp,dpinv);
    Warp->pRondp(p,dpinv,p);

    iteration++;
    iterationGlobale++;

    computeEvalRMS(p);

    //        std::cout << p.t() << std::endl;
  }
  while( (!diverge) && (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
  //while( (!diverge) && (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );

  nbIteration=iteration;

  if(diverge)
  {
    if(computeCovariance){
      covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
      covarianceMatrix = -1;
      MI_postEstimation = -1;
      NMI_postEstimation = -1;
    }
    deletePosEvalRMS();

    //        throw(vpTrackingException(vpTrackingException::badValue, "Tracking failed")) ;
  }
  else
  {
    MI_postEstimation=-getCost(I,p);
    NMI_postEstimation=-getNormalizedCost(I,p);
    //        std::cout << "MI apres: " << MI_postEstimation << std::endl;
    //        std::cout << "NMI apres: " << NMI_postEstimation << std::endl;
    if(MI_preEstimation>MI_postEstimation)
    {
      p=p_avant_estimation;
      MI_postEstimation = MI_preEstimation;
      NMI_postEstimation = NMI_preEstimation;
      covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
      covarianceMatrix = -1;
    }

    deletePosEvalRMS();

    if(computeCovariance){
      try{
        covarianceMatrix = (-H).inverseByLU();
        //            covarianceMatrix = (-Hnorm).inverseByLU();
      }
      catch(...){
        covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
        covarianceMatrix = -1;
        MI_postEstimation = -1;
        NMI_postEstimation = -1;
        deletePosEvalRMS();
        //            throw(vpMatrixException(vpMatrixException::matrixError, "Covariance not inversible")) ;
      }
    }
  }
}
Ejemplo n.º 14
0
void vpTemplateTrackerMIESM::trackNoPyr(const vpImage<unsigned char> &I)
{
  if(!CompoInitialised)
    std::cout<<"Compositionnal tracking no initialised\nUse initCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
  dW=0;

  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);
  vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
  vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
  /*	if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 && ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
  {
    getGradX(dIx, d2Ix,fgdG,taillef);
    getGradY(dIx, d2Ixy,fgdG,taillef);
    getGradY(dIy, d2Iy,fgdG,taillef);
  }*/

  double erreur=0;
  int Nbpoint=0;
  int point;

  MI_preEstimation=-getCost(I,p);

  double lambda=lambdaDep;
  double MI=0;
  //double MIprec=-1000;

  double i2,j2;
  double Tij;
  double IW;
  //int cr,ct;
  //double er,et;

  vpColVector dpinv(nbParam);

  double dx,dy;
  double alpha=2.;

  int i,j;
  unsigned int iteration=0;
  do
  {
    Nbpoint=0;
    //MIprec=MI;
    MI=0;
    erreur=0;

    zeroProbabilities();

    /////////////////////////////////////////////////////////////////////////
    // Inverse
    Warp->computeCoeff(p);
    for(point=0;point<(int)templateSize;point++)
    {
      i=ptTemplate[point].y;
      j=ptTemplate[point].x;
      X1[0]=j;X1[1]=i;

      Warp->computeDenom(X1,p);
      Warp->warpX(X1,X2,p);

      j2=X2[0];i2=X2[1];

      if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
      {
        Nbpoint++;
        Tij=ptTemplate[point].val;
        if(!blur)
          IW=I.getValue(i2,j2);
        else
          IW=BI.getValue(i2,j2);

        //ct=ptTemplateSupp[point].ct;
        //et=ptTemplateSupp[point].et;
        //cr=(int)((IW*(Nc-1))/255.);
        //er=((double)IW*(Nc-1))/255.-cr;

        //                if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) // cas à tester AY
        //                    vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
        //                else
        //                    vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
      }
    }

    if(Nbpoint==0)
    {
      //std::cout<<"plus de point dans template suivi"<<std::endl;
      diverge=true;
      MI=0;
      throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
    }
    else
    {
      computeProba(Nbpoint);
      computeMI(MI);
      if(hessianComputation!= vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
        computeHessien(HInverse);
      computeGradient();
      GInverse=G;

      /////////////////////////////////////////////////////////////////////////
      // DIRECT

      Nbpoint=0;
      //MIprec=MI;
      MI=0;
      erreur=0;

      zeroProbabilities();

      Warp->computeCoeff(p);
#ifdef VISP_HAVE_OPENMP
      int nthreads = omp_get_num_procs() ;
      //std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
      omp_set_num_threads(nthreads);
#pragma omp parallel for private(point, Tij,IW,i,j,i2,j2,/*cr,ct,er,et,*/dx,dy) default(shared)
#endif
      for(point=0;point<(int)templateSize;point++)
      {
        i=ptTemplate[point].y;
        j=ptTemplate[point].x;
        X1[0]=j;X1[1]=i;
        Warp->computeDenom(X1,p);
        Warp->warpX(i,j,i2,j2,p);
        X2[0]=j2;X2[1]=i2;

        //Warp->computeDenom(X1,p);
        if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
        {
          Nbpoint++;
          Tij=ptTemplate[point].val;
          //Tij=Iterateurvecteur->val;
          if(!blur)
            IW=I.getValue(i2,j2);
          else
            IW=BI.getValue(i2,j2);

          dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
          dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;

          //ct=(int)((IW*(Nc-1))/255.);
          //et=((double)IW*(Nc-1))/255.-ct;
          //cr=ptTemplateSupp[point].ct;
          //er=ptTemplateSupp[point].et;

          Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);

          double *tptemp=new double[nbParam];
          for(unsigned int it=0;it<nbParam;it++)
            tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;


          //calcul de l'erreur
          erreur+=(Tij-IW)*(Tij-IW);
          //                    if(bspline==3)
          //                        vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
          //                    else
          //                        vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
          //					vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,
          //                               hessianComputation==vpHessienType::USE_HESSIEN_DESIRE);

          delete[] tptemp;

        }
      }

      computeProba(Nbpoint);
      computeMI(MI);
      if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
        computeHessien(HDirect);
      computeGradient();
      GDirect=G;

      if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
      {
        H=HDirect+HInverse;
        vpMatrix::computeHLM(H,lambda,HLM);
      }
      G=GDirect-GInverse;
      //G=GDirect;
      //G=-GInverse;

      try
      {
        if(minimizationMethod==vpTemplateTrackerMIESM::USE_GRADIENT)
          dp=-gain*0.3*G;
        else
        {
          switch(hessianComputation)
          {
          case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
            dp=gain*HLMdesireInverse*G;
            break;
          case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
            if(HLM.cond()>HLMdesire.cond())
              dp=gain*HLMdesireInverse*G;
            else
              dp=gain*0.3*HLM.inverseByLU()*G;
            break;
          default:
            dp=gain*0.3*HLM.inverseByLU()*G;
            break;
          }
        }
      }
      catch(vpException &e)
      {
        //std::cerr<<"probleme inversion"<<std::endl;
        throw(e);
      }

      if(ApproxHessian==HESSIAN_NONSECOND)
        dp=-1.*dp;

      if(useBrent)
      {
        alpha=2.;
        computeOptimalBrentGain(I,p,-MI,dp,alpha);
        dp=alpha*dp;
      }
      p+=dp;

      iteration++;
    }
  }
  while( /*(MI!=MIprec) &&*/(iteration< iterationMax));

  MI_postEstimation=-getCost(I,p);
  if(MI_preEstimation>MI_postEstimation)
  {
    MI_postEstimation = -1;
  }

  nbIteration=iteration;
}
void vpTemplateTrackerMIForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
{
  dW=0;

  double erreur=0;
  int Nbpoint=0;
  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);
  vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
  vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);

  double MI=0,MIprec=-1000;

  MI_preEstimation=-getCost(I,p);

  double i2,j2;
  double Tij;
  double IW,dx,dy;
  //unsigned
  int cr,ct;
  double er,et;
  double alpha=2.;

  int i,j;
  unsigned int iteration=0;

  initPosEvalRMS(p);
  do
  {
    if(iteration%5==0)
      initHessienDesired(I);
    Nbpoint=0;
    MIprec=MI;
    MI=0;
    erreur=0;

    zeroProbabilities();

    Warp->computeCoeff(p);
#ifdef VISP_HAVE_OPENMP
    int nthreads = omp_get_num_procs() ;
    //std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
    omp_set_num_threads(nthreads);
#pragma omp parallel for private(Tij,IW,i,j,i2,j2,cr,ct,er,et,dx,dy) default(shared)
#endif
    for(int point=0;point<(int)templateSize;point++)
    {
      i=ptTemplate[point].y;
      j=ptTemplate[point].x;
      X1[0]=j;X1[1]=i;

      Warp->computeDenom(X1,p);
      Warp->warpX(X1,X2,p);

      j2=X2[0];i2=X2[1];

      if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
      {
        Nbpoint++;
        Tij=ptTemplate[point].val;
        //Tij=Iterateurvecteur->val;
        if(!blur)
          IW=I.getValue(i2,j2);
        else
          IW=BI.getValue(i2,j2);

        dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
        dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;

        ct=(int)((IW*(Nc-1))/255.);
        cr=(int)((Tij*(Nc-1))/255.);
        et=(IW*(Nc-1))/255.-ct;
        er=((double)Tij*(Nc-1))/255.-cr;

        //calcul de l'erreur
        erreur+=(Tij-IW)*(Tij-IW);

        //Calcul de l'histogramme joint par interpolation bilinÈaire (Bspline ordre 1)
        Warp->dWarp(X1,X2,p,dW);

        //double *tptemp=temp;
        double *tptemp=new double[nbParam];;
        for(unsigned int it=0;it<nbParam;it++)
          tptemp[it] =(dW[0][it]*dx+dW[1][it]*dy);
        //*tptemp++ =dW[0][it]*dIWx+dW[1][it]*dIWy;
        //std::cout<<cr<<"   "<<ct<<"  ; ";
        if(ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
          vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
        else if(ApproxHessian==HESSIAN_0 || ApproxHessian==HESSIAN_NEW)
          vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);

        delete[] tptemp;
      }
    }

    if(Nbpoint==0)
    {
      //std::cout<<"plus de point dans template suivi"<<std::endl;
      diverge=true;
      MI=0;
      deletePosEvalRMS();
      throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
    }
    else
    {
      computeProba(Nbpoint);
      computeMI(MI);
      //std::cout<<iteration<<"\tMI= "<<MI<<std::endl;
      computeHessien(H);
      computeGradient();

      vpMatrix::computeHLM(H,lambda,HLM);
      try
      {
        switch(hessianComputation)
        {
        case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
          dp=gain*HLMdesireInverse*G;
          break;
        case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
          if(HLM.cond()>HLMdesire.cond())
            dp=gain*HLMdesireInverse*G;
          else
            dp=gain*0.2*HLM.inverseByLU()*G;
          break;
        default:
          dp=gain*0.2*HLM.inverseByLU()*G;
          break;
        }
      }
      catch(vpException &e)
      {
        //std::cerr<<"probleme inversion"<<std::endl;
        deletePosEvalRMS();
        throw(e);
      }
    }

    switch(minimizationMethod)
    {
    case vpTemplateTrackerMIForwardAdditional::USE_LMA:
    {
      vpColVector p_test_LMA(nbParam);
      if(ApproxHessian==HESSIAN_NONSECOND)
        p_test_LMA=p-100000.1*dp;
      else
        p_test_LMA=p+1.*dp;
      MI=-getCost(I,p);
      double MI_LMA=-getCost(I,p_test_LMA);
      if(MI_LMA>MI)
      {
        p=p_test_LMA;
        lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
      }
      else
      {
        lambda=(lambda*10.<1e6)?1e6:lambda*10.;
      }
    }
      break;
    case vpTemplateTrackerMIForwardAdditional::USE_GRADIENT:
    {
      dp=-gain*6.0*G;
      if(useBrent)
      {
        alpha=2.;
        computeOptimalBrentGain(I,p,-MI,dp,alpha);
        dp=alpha*dp;
      }
      p+=1.*dp;
      break;
    }

    case vpTemplateTrackerMIForwardAdditional::USE_QUASINEWTON:
    {
      double s_scal_y;
      if(iterationGlobale!=0)
      {
        vpColVector s_quasi=p-p_prec;
        vpColVector y_quasi=G-G_prec;
        s_scal_y=s_quasi.t()*y_quasi;
        //if(s_scal_y!=0)//BFGS
        //	KQuasiNewton=KQuasiNewton-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y;
        //if(s_scal_y!=0)//DFP
        if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())
          KQuasiNewton=KQuasiNewton+0.001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
      }
      dp=-KQuasiNewton*G;
      p_prec=p;
      G_prec=G;
      p-=1.01*dp;
    }
      break;

    default:
    {
      if(ApproxHessian==HESSIAN_NONSECOND)
        dp=-0.1*dp;
      if(useBrent)
      {
        alpha=2.;
        computeOptimalBrentGain(I,p,-MI,dp,alpha);
        //std::cout<<alpha<<std::endl;
        dp=alpha*dp;
      }

      p+=1.*dp;
      break;
    }
    }

    computeEvalRMS(p);
    iteration++;
    iterationGlobale++;

  }
  while( (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
  //while( (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
  if(Nbpoint==0) {
    //std::cout<<"plus de point dans template suivi"<<std::endl;
    deletePosEvalRMS();
    throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
  }

  nbIteration=iteration;
  MI_postEstimation=-getCost(I,p);
  if(MI_preEstimation>MI_postEstimation)
  {
    MI_postEstimation = -1;
  }
  deletePosEvalRMS();
}
Ejemplo n.º 16
0
void TetMesh::interpolateGradient(int element, const double * U, int numFields, Vec3d pos, double * grad) const
{
  computeGradient(element, U, numFields, grad);
}
Ejemplo n.º 17
0
double Gradient::computeGradient(dVector& vecGradient, Model* m, DataSet* X)
{
  double ans = 0.0;
  
#ifdef _OPENMP
  if( nbThreadsMP < 1 )
    nbThreadsMP = omp_get_max_threads();
  setMaxNumberThreads(nbThreadsMP);
  pInfEngine->setMaxNumberThreads(nbThreadsMP);
  pFeatureGen->setMaxNumberThreads(nbThreadsMP);
#endif
  
  //Check the size of vecGradient
  int nbFeatures = pFeatureGen->getNumberOfFeatures();
  if(vecGradient.getLength() != nbFeatures)
    vecGradient.create(nbFeatures);
  else
    vecGradient.set(0);
  
  ////////////////////////////////////////////////////////////
  // Start of parallel Region
  // Some weird stuff in gcc 4.1, with openmp 2.5 support
  //
  // Note 1: In OpenMP 2.5, the iteration variable in "for" must be
  // a signed integer variable type. In OpenMP 3.0 (_OPENMP>=200805),
  // it may  also be an unsigned integer variable type, a pointer type,
  // or a constant-time random access iterator type.
  //
  // Note 2: schedule(static | dynamic): In the dynamic schedule, there
  // is no predictable order in which the loop items are assigned to
  // different threads. Each thread asks the OpenMP runtime library for
  // an iteration number, then handles it, then asks for the next one.
  // It is thus useful when different iterations in the loop may take
  // different time to execute.
#pragma omp parallel default(none) \
  shared(vecGradient, X, m, ans, nbFeatures, std::cout)
  {
    // code inside this region runs in parallel
    dVector g(nbFeatures, COLVECTOR, 0.0);
    
#pragma omp for schedule(dynamic) reduction(+:ans)
    for(int i=0; (int)i<X->size(); i++) {
      DataSequence* x = X->at(i);
      if( m->isWeightSequence() && x->getWeightSequence() != 1.0) {
        dVector tmp(nbFeatures, COLVECTOR, 0.0);
        ans += computeGradient(tmp, m, x) * x->getWeightSequence();
        tmp.multiply(x->getWeightSequence());
        g.add(tmp);
      }
      else {
        ans += computeGradient(g, m, x);
      }
    }
    
    // We now put togheter the gradients
    // No two threads can execute a critical directive of the same name at the same time
#pragma omp critical (reduce_sum)
    {
      vecGradient.add(g);
    }
  }
  // End of parallel Region
  ////////////////////////////////////////////////////////////
  vecGradient.negate();
  
  // MaxMargin objective: min L = 0.5*\L2sigma*W*W + Loss()
  // MLE objective: min L = 0.5*1/(\L2sigma*\L2sigma)*W*W - log p(y|x)
  
  // Add the regularization term
  double scale = (m->isMaxMargin())
		? m->getRegL2Sigma()
		: 1/(double)(m->getRegL2Sigma()*m->getRegL2Sigma());
  
  if( m->isMaxMargin() )
    ans = (1/(double)X->size()) * ans;
  
  if(m->getRegL2Sigma()!=0.0f)
  {
    for(int f=0; f<nbFeatures; f++)
      vecGradient[f] += (*m->getWeights())[f]*scale;
    ans += 0.5*scale*m->getWeights()->l2Norm(false);
  }
  
  return ans;
}