Skip to content

Commit

Permalink
Working on visp openmp directive
Browse files Browse the repository at this point in the history
  • Loading branch information
@SamFlt
SamFlt committed Sep 18, 2024
1 parent aee5e20 commit c604448
Show file tree
Hide file tree
Showing 3 changed files with 101 additions and 97 deletions.
6 changes: 6 additions & 0 deletions cmake/templates/vpConfig.h.in
View file
Original file line number Diff line number Diff line change
Expand Up @@ -526,6 +526,12 @@ namespace vp = VISP_NAMESPACE_NAME;
// Defined if we want to use openmp
#cmakedefine VISP_HAVE_OPENMP

#if defined(VISP_HAVE_OPENMP)
// Wrapper around OpenMP pragma clauses that disables them if openMP is disabled. This clause should not appear in headers
#define VISP_OPENMP(clause) \
_Pragma(omp clause)
#endif

// Defined if nlohmann json parser is found
#cmakedefine VISP_HAVE_NLOHMANN_JSON

Expand Down
2 changes: 1 addition & 1 deletion modules/tracker/rbt/src/core/vpRBTracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -587,7 +587,7 @@ void vpRBTracker::loadConfiguration(const nlohmann::json &j)
nlohmann::json driftSettings = j.at("drift");
m_driftDetector = factory.buildFromJson(driftSettings);
if (m_driftDetector == nullptr) {
throw vpException(vpException::badValue, "Cannot instanciate drift detection with the current settings, make sure that the type is registered in the factory");
throw vpException(vpException::badValue, "Cannot instantiate drift detection with the current settings, make sure that the type is registered in the factory");
}
}
}
Expand Down
190 changes: 94 additions & 96 deletions modules/tracker/rbt/src/features/vpRBSilhouetteCCDTracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -384,122 +384,120 @@ void vpRBSilhouetteCCDTracker::computeLocalStatistics(const vpImage<vpRGBa> &I,
}

}
#ifdef VISP_HAVE_OPENMP
#pragma omp parallel for
#endif
for (unsigned int i = 0; i < resolution; ++i) {
if (!m_controlPoints[i].isValid()) {
continue;
}
VISP_OPENMP(parallel for)
for (unsigned int i = 0; i < resolution; ++i) {
if (!m_controlPoints[i].isValid()) {
continue;
}

int k = 0;
// w1 = \sum wp_1, w2 = \sum wp_2
double w1 = 0.0, w2 = 0.0;
int k = 0;
// w1 = \sum wp_1, w2 = \sum wp_2
double w1 = 0.0, w2 = 0.0;

// store mean value near the curve
std::array<double, 3> m1 { 0.0, 0.0, 0.0 }, m2 { 0.0, 0.0, 0.0 };
// store mean value near the curve
std::array<double, 3> m1 { 0.0, 0.0, 0.0 }, m2 { 0.0, 0.0, 0.0 };

// store the second mean value near the curve
std::array<double, 9> m1_o2 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
std::array<double, 9> m2_o2 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
// store the second mean value near the curve
std::array<double, 9> m1_o2 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
std::array<double, 9> m2_o2 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };

// compute local statistics
// compute local statistics

// start search the points in the +n direction as well as -n direction
double wp1 = 0.0, wp2 = 0.0;
// start search the points in the +n direction as well as -n direction
double wp1 = 0.0, wp2 = 0.0;

double *vic_ptr = stats.vic[i];
double *mean_vic_ptr = stats.mean_vic[i];
double *cov_vic_ptr = stats.cov_vic[i];
double *pix_ptr = stats.imgPoints[i];
double *weight_ptr = stats.weight[i];
double *vic_ptr = stats.vic[i];
double *mean_vic_ptr = stats.mean_vic[i];
double *cov_vic_ptr = stats.cov_vic[i];
double *pix_ptr = stats.imgPoints[i];
double *weight_ptr = stats.weight[i];

for (int j = m_ccdParameters.delta_h; j <= m_ccdParameters.h; j += m_ccdParameters.delta_h, k++) {
wp1 = 0.0, wp2 = 0.0;
int negative_normal = k + (int)floor(m_ccdParameters.h / m_ccdParameters.delta_h);
const double *vic_k = vic_ptr + 10 * k;
for (int j = m_ccdParameters.delta_h; j <= m_ccdParameters.h; j += m_ccdParameters.delta_h, k++) {
wp1 = 0.0, wp2 = 0.0;
int negative_normal = k + (int)floor(m_ccdParameters.h / m_ccdParameters.delta_h);
const double *vic_k = vic_ptr + 10 * k;

// wp1 = w(a_{k,l})*w(d_{k,l})*w(d)
wp1 = (vic_k[5] * vic_k[7] / normalized_param[i][0]);
// wp1 = w(a_{k,l})*w(d_{k,l})*w(d)
wp1 = (vic_k[5] * vic_k[7] / normalized_param[i][0]);

// wp2 = w(a_{k,l})*w(d_{k,l})*w(d)
wp2 = (vic_k[6] * vic_k[7] / normalized_param[i][1]);
//w1 = \sum{wp1}
w1 += wp1;
// wp2 = w(a_{k,l})*w(d_{k,l})*w(d)
wp2 = (vic_k[6] * vic_k[7] / normalized_param[i][1]);
//w1 = \sum{wp1}
w1 += wp1;

//w2 = \sum{wp2}
w2 += wp2;
//w2 = \sum{wp2}
w2 += wp2;

// compute the mean value in the vicinity of a point
// m_{ks} = I{k}^{s} = \sum_{l} w_{kls}{I_{kl}} : s = 1 or 2
const vpRGBa pixelRGBa = I(vic_k[0], vic_k[1]);
double *pixel = pix_ptr + k * 3;
pixel[0] = pixelRGBa.R;
pixel[1] = pixelRGBa.G;
pixel[2] = pixelRGBa.B;
// compute the mean value in the vicinity of a point
// m_{ks} = I{k}^{s} = \sum_{l} w_{kls}{I_{kl}} : s = 1 or 2
const vpRGBa pixelRGBa = I(vic_k[0], vic_k[1]);
double *pixel = pix_ptr + k * 3;
pixel[0] = pixelRGBa.R;
pixel[1] = pixelRGBa.G;
pixel[2] = pixelRGBa.B;

m1[0] += wp1 * pixel[0];
m1[1] += wp1 * pixel[1];
m1[2] += wp1 * pixel[2];
m1[0] += wp1 * pixel[0];
m1[1] += wp1 * pixel[1];
m1[2] += wp1 * pixel[2];

m2[0] += wp2 * pixel[0];
m2[1] += wp2 * pixel[1];
m2[2] += wp2 * pixel[2];
m2[0] += wp2 * pixel[0];
m2[1] += wp2 * pixel[1];
m2[2] += wp2 * pixel[2];


// compute second order local statistics
// m_{k,s} = \sum_{l} w_{kls} I_{kl}*I_{kl}^T
for (unsigned int m = 0; m < 3; ++m) {
for (unsigned int n = 0; n < 3; ++n) {
m1_o2[m * 3 + n] += wp1 * pixel[m] * pixel[n];
m2_o2[m * 3 + n] += wp2 * pixel[m] * pixel[n];
// compute second order local statistics
// m_{k,s} = \sum_{l} w_{kls} I_{kl}*I_{kl}^T
for (unsigned int m = 0; m < 3; ++m) {
for (unsigned int n = 0; n < 3; ++n) {
m1_o2[m * 3 + n] += wp1 * pixel[m] * pixel[n];
m2_o2[m * 3 + n] += wp2 * pixel[m] * pixel[n];
}
}
}
const double *vic_neg = vic_ptr + 10 * negative_normal;
const vpRGBa pixelNegRGBa = I(vic_neg[0], vic_neg[1]);
double *pixelNeg = pix_ptr + negative_normal * 3;

pixelNeg[0] = pixelNegRGBa.R;
pixelNeg[1] = pixelNegRGBa.G;
pixelNeg[2] = pixelNegRGBa.B;
wp1 = (vic_neg[5] * vic_neg[7] / normalized_param[i][0]);
wp2 = (vic_neg[6] * vic_neg[7] / normalized_param[i][1]);
w1 += wp1;
w2 += wp2;

m1[0] += wp1 * pixelNeg[0];
m1[1] += wp1 * pixelNeg[1];
m1[2] += wp1 * pixelNeg[2];

m2[0] += wp2 * pixelNeg[0];
m2[1] += wp2 * pixelNeg[1];
m2[2] += wp2 * pixelNeg[2];

for (int m = 0; m < 3; ++m) {
for (int n = 0; n < 3; ++n) {
m1_o2[m * 3 + n] += wp1 * pixelNeg[m] * pixelNeg[n];
m2_o2[m * 3 + n] += wp2 * pixelNeg[m] * pixelNeg[n];
const double *vic_neg = vic_ptr + 10 * negative_normal;
const vpRGBa pixelNegRGBa = I(vic_neg[0], vic_neg[1]);
double *pixelNeg = pix_ptr + negative_normal * 3;

pixelNeg[0] = pixelNegRGBa.R;
pixelNeg[1] = pixelNegRGBa.G;
pixelNeg[2] = pixelNegRGBa.B;
wp1 = (vic_neg[5] * vic_neg[7] / normalized_param[i][0]);
wp2 = (vic_neg[6] * vic_neg[7] / normalized_param[i][1]);
w1 += wp1;
w2 += wp2;

m1[0] += wp1 * pixelNeg[0];
m1[1] += wp1 * pixelNeg[1];
m1[2] += wp1 * pixelNeg[2];

m2[0] += wp2 * pixelNeg[0];
m2[1] += wp2 * pixelNeg[1];
m2[2] += wp2 * pixelNeg[2];

for (int m = 0; m < 3; ++m) {
for (int n = 0; n < 3; ++n) {
m1_o2[m * 3 + n] += wp1 * pixelNeg[m] * pixelNeg[n];
m2_o2[m * 3 + n] += wp2 * pixelNeg[m] * pixelNeg[n];
}
}
}
}
mean_vic_ptr[0] = m1[0] / w1;
mean_vic_ptr[1] = m1[1] / w1;
mean_vic_ptr[2] = m1[2] / w1;

mean_vic_ptr[3] = m2[0] / w2;
mean_vic_ptr[4] = m2[1] / w2;
mean_vic_ptr[5] = m2[2] / w2;

for (unsigned int m = 0; m < 3; ++m) {
for (unsigned int n = 0; n < 3; ++n) {
cov_vic_ptr[m * 3 + n] = m1_o2[m * 3 + n] / w1 - m1[m] * m1[n] / (w1 * w1);
cov_vic_ptr[9 + m * 3 + n] = m2_o2[m * 3 + n] / w2 - m2[m] * m2[n] / (w2 * w2);
mean_vic_ptr[0] = m1[0] / w1;
mean_vic_ptr[1] = m1[1] / w1;
mean_vic_ptr[2] = m1[2] / w1;

mean_vic_ptr[3] = m2[0] / w2;
mean_vic_ptr[4] = m2[1] / w2;
mean_vic_ptr[5] = m2[2] / w2;

for (unsigned int m = 0; m < 3; ++m) {
for (unsigned int n = 0; n < 3; ++n) {
cov_vic_ptr[m * 3 + n] = m1_o2[m * 3 + n] / w1 - m1[m] * m1[n] / (w1 * w1);
cov_vic_ptr[9 + m * 3 + n] = m2_o2[m * 3 + n] / w2 - m2[m] * m2[n] / (w2 * w2);
}
cov_vic_ptr[m * 3 + m] += m_ccdParameters.kappa;
cov_vic_ptr[9 + m * 3 + m] += m_ccdParameters.kappa;
}
cov_vic_ptr[m * 3 + m] += m_ccdParameters.kappa;
cov_vic_ptr[9 + m * 3 + m] += m_ccdParameters.kappa;
}

}
}
}

void vpRBSilhouetteCCDTracker::computeErrorAndInteractionMatrix()
Expand Down

0 comments on commit c604448

Please sign in to comment.