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;
}
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] );
}
}
}
}
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] );
}
}
}
}
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;
}
// 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;
}
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;
}
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;
}
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);
}
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")) ;
}
}
}
}
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();
}
void TetMesh::interpolateGradient(int element, const double * U, int numFields, Vec3d pos, double * grad) const
{
computeGradient(element, U, numFields, grad);
}
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;
}
