Skip to content
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
58 changes: 30 additions & 28 deletions modules/visual_features/src/visual-feature/vpFeatureMomentBasic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,25 +48,25 @@ BEGIN_VISP_NAMESPACE
vpFeatureMomentBasic::vpFeatureMomentBasic(vpMomentDatabase &data_base, double A_, double B_, double C_,
vpFeatureMomentDatabase *featureMoments)
: vpFeatureMoment(data_base, A_, B_, C_, featureMoments), order(0)
{}
{ }

/*!
* Computes interaction matrix for basic moment. Called internally.
* The moment primitives must be computed before calling this.
*/
void vpFeatureMomentBasic::compute_interaction()
{
unsigned int delta;
double delta;
const vpMomentObject &m = moment->getObject();
order = m.getOrder() + 1;
interaction_matrices.resize(order * order);
for (std::vector<vpMatrix>::iterator i = interaction_matrices.begin(); i != interaction_matrices.end(); ++i)
i->resize(1, 6);
if (m.getType() == vpMomentObject::DISCRETE) {
delta = 0;
delta = 0.;
}
else {
delta = 1;
delta = 1.;
}

int VX = 0;
Expand All @@ -80,68 +80,70 @@ void vpFeatureMomentBasic::compute_interaction()
interaction_matrices[0][0][VX] = -delta * A * m.get(0, 0);
interaction_matrices[0][0][VY] = -delta * B * m.get(0, 0);
interaction_matrices[0][0][VZ] =
3 * delta * (A * m.get(1, 0) + B * m.get(0, 1) + C * m.get(0, 0)) - delta * C * m.get(0, 0);
3. * delta * (A * m.get(1, 0) + B * m.get(0, 1) + C * m.get(0, 0)) - delta * C * m.get(0, 0);

interaction_matrices[0][0][WX] = 3 * delta * m.get(0, 1);
interaction_matrices[0][0][WY] = -(3 * delta * m.get(1, 0));
interaction_matrices[0][0][WZ] = 0;
interaction_matrices[0][0][WX] = 3. * delta * m.get(0, 1);
interaction_matrices[0][0][WY] = -(3. * delta * m.get(1, 0));
interaction_matrices[0][0][WZ] = 0.;

// int i=0;
const unsigned int order_u = static_cast<unsigned int>(order);
for (unsigned int j = 1; j < order_u - 1; j++) {
unsigned int j_ = static_cast<unsigned int>(j);
double j_double = static_cast<double>(j);
unsigned int jm1_ = j_ - 1;
unsigned int jp1_ = j_ + 1;

interaction_matrices[j_ * order][0][VX] = -delta * A * m.get(0, j_);
interaction_matrices[j_ * order][0][VY] =
-j * (A * m.get(1, jm1_) + B * m.get(0, j_) + C * m.get(0, jm1_)) - delta * B * m.get(0, j_);
-j_double * (A * m.get(1, jm1_) + B * m.get(0, j_) + C * m.get(0, jm1_)) - delta * B * m.get(0, j_);
interaction_matrices[j_ * order][0][VZ] =
(j + 3 * delta) * (A * m.get(1, j_) + B * m.get(0, jp1_) + C * m.get(0, j_)) - delta * C * m.get(0, j_);
(j_double + 3. * delta) * (A * m.get(1, j_) + B * m.get(0, jp1_) + C * m.get(0, j_)) - delta * C * m.get(0, j_);

interaction_matrices[j_ * order][0][WX] = (j + 3 * delta) * m.get(0, jp1_) + j * m.get(0, jm1_);
interaction_matrices[j_ * order][0][WY] = -(j + 3 * delta) * m.get(1, j_);
interaction_matrices[j_ * order][0][WZ] = -j * m.get(1, jm1_);
interaction_matrices[j_ * order][0][WX] = (j_double + 3. * delta) * m.get(0, jp1_) + j_double * m.get(0, jm1_);
interaction_matrices[j_ * order][0][WY] = -(j_double + 3. * delta) * m.get(1, j_);
interaction_matrices[j_ * order][0][WZ] = -j_double * m.get(1, jm1_);
}

// int j=0;
for (unsigned int i = 1; i < order_u - 1; i++) {
unsigned int i_ = static_cast<unsigned int>(i);
double i_double = static_cast<double>(i);
unsigned int im1_ = i_ - 1;
unsigned int ip1_ = i_ + 1;

interaction_matrices[i_][0][VX] =
-i * (A * m.get(i_, 0) + B * m.get(im1_, 1) + C * m.get(im1_, 0)) - delta * A * m.get(i_, 0);
-i_double * (A * m.get(i_, 0) + B * m.get(im1_, 1) + C * m.get(im1_, 0)) - delta * A * m.get(i_, 0);
interaction_matrices[i_][0][VY] = -delta * B * m.get(i_, 0);
interaction_matrices[i_][0][VZ] =
(i + 3 * delta) * (A * m.get(ip1_, 0) + B * m.get(i_, 1) + C * m.get(i_, 0)) - delta * C * m.get(i_, 0);
(i_double + 3. * delta) * (A * m.get(ip1_, 0) + B * m.get(i_, 1) + C * m.get(i_, 0)) - delta * C * m.get(i_, 0);

interaction_matrices[i_][0][WX] = (i + 3 * delta) * m.get(i_, 1);
interaction_matrices[i_][0][WY] = -(i + 3 * delta) * m.get(ip1_, 0) - i * m.get(im1_, 0);
interaction_matrices[i_][0][WZ] = i * m.get(im1_, 1);
interaction_matrices[i_][0][WX] = (i_double + 3. * delta) * m.get(i_, 1);
interaction_matrices[i_][0][WY] = -(i_double + 3. * delta) * m.get(ip1_, 0) - i_double * m.get(im1_, 0);
interaction_matrices[i_][0][WZ] = i_double * m.get(im1_, 1);
}

for (unsigned int j = 1; j < order_u - 1; j++) {
unsigned int j_ = static_cast<unsigned int>(j);
for (unsigned int j_ = 1; j_ < order_u - 1; j_++) {
unsigned int jm1_ = j_ - 1;
unsigned int jp1_ = j_ + 1;

for (unsigned int i = 1; i < order_u - j - 1; i++) {
unsigned int i_ = static_cast<unsigned int>(i);
for (unsigned int i_ = 1; i_ < order_u - j_ - 1; i_++) {
unsigned int im1_ = i_ - 1;
unsigned int ip1_ = i_ + 1;
double i_double = static_cast<double>(i_);
double j_double = static_cast<double>(j_);

interaction_matrices[j_ * order + i_][0][VX] =
-i * (A * m.get(i_, j_) + B * m.get(im1_, jp1_) + C * m.get(im1_, j_)) - delta * A * m.get(i_, j_);
-i_double * (A * m.get(i_, j_) + B * m.get(im1_, jp1_) + C * m.get(im1_, j_)) - delta * A * m.get(i_, j_);
interaction_matrices[j_ * order + i_][0][VY] =
-j * (A * m.get(ip1_, jm1_) + B * m.get(i_, j_) + C * m.get(i_, jm1_)) - delta * B * m.get(i_, j_);
-j_double * (A * m.get(ip1_, jm1_) + B * m.get(i_, j_) + C * m.get(i_, jm1_)) - delta * B * m.get(i_, j_);
interaction_matrices[j_ * order + i_][0][VZ] =
(i + j + 3 * delta) * (A * m.get(ip1_, j_) + B * m.get(i_, jp1_) + C * m.get(i_, j_)) -
(i_double + j_double + 3. * delta) * (A * m.get(ip1_, j_) + B * m.get(i_, jp1_) + C * m.get(i_, j_)) -
delta * C * m.get(i_, j_);

interaction_matrices[j_ * order + i_][0][WX] = (i + j + 3 * delta) * m.get(i_, jp1_) + j * m.get(i_, jm1_);
interaction_matrices[j_ * order + i_][0][WY] = -(i + j + 3 * delta) * m.get(ip1_, j_) - i * m.get(im1_, j_);
interaction_matrices[j_ * order + i_][0][WZ] = i * m.get(im1_, jp1_) - j * m.get(ip1_, jm1_);
interaction_matrices[j_ * order + i_][0][WX] = (i_double + j_double + 3. * delta) * m.get(i_, jp1_) + j_double * m.get(i_, jm1_);
interaction_matrices[j_ * order + i_][0][WY] = -(i_double + j_double + 3. * delta) * m.get(ip1_, j_) - i_double * m.get(im1_, j_);
interaction_matrices[j_ * order + i_][0][WZ] = i_double * m.get(im1_, jp1_) - j_double * m.get(ip1_, jm1_);
}
}
}
Expand Down
Loading