Visual Servoing Platform  version 3.6.1 under development (2024-06-23)
vpFeatureMomentBasic.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Implementation for all supported moment features.
32  */
33 
34 #include <vector>
35 #include <visp3/core/vpMomentObject.h>
36 #include <visp3/visual_features/vpFeatureMomentBasic.h>
37 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
38 
48  vpFeatureMomentBasic::vpFeatureMomentBasic(vpMomentDatabase &data_base, double A_, double B_, double C_,
49  vpFeatureMomentDatabase *featureMoments)
50  : vpFeatureMoment(data_base, A_, B_, C_, featureMoments), order(0)
51 { }
52 
58 {
59  int delta;
60  const vpMomentObject &m = moment->getObject();
61  order = m.getOrder() + 1;
63  for (std::vector<vpMatrix>::iterator i = interaction_matrices.begin(); i != interaction_matrices.end(); ++i)
64  i->resize(1, 6);
65  if (m.getType() == vpMomentObject::DISCRETE) {
66  delta = 0;
67  }
68  else {
69  delta = 1;
70  }
71 
72  int VX = 0;
73  int VY = 1;
74  int VZ = 2;
75  int WX = 3;
76  int WY = 4;
77  int WZ = 5;
78 
79  // i=0;j=0
80  interaction_matrices[0][0][VX] = -delta * A * m.get(0, 0);
81  interaction_matrices[0][0][VY] = -delta * B * m.get(0, 0);
82  interaction_matrices[0][0][VZ] =
83  3 * delta * (A * m.get(1, 0) + B * m.get(0, 1) + C * m.get(0, 0)) - delta * C * m.get(0, 0);
84 
85  interaction_matrices[0][0][WX] = 3 * delta * m.get(0, 1);
86  interaction_matrices[0][0][WY] = -3 * delta * m.get(1, 0);
87  interaction_matrices[0][0][WZ] = 0;
88 
89  // int i=0;
90  for (int j = 1; j < (int)order - 1; j++) {
91  unsigned int j_ = (unsigned int)j;
92  unsigned int jm1_ = j_ - 1;
93  unsigned int jp1_ = j_ + 1;
94 
95  interaction_matrices[j_ * order][0][VX] = -delta * A * m.get(0, j_);
96  interaction_matrices[j_ * order][0][VY] =
97  -j * (A * m.get(1, jm1_) + B * m.get(0, j_) + C * m.get(0, jm1_)) - delta * B * m.get(0, j_);
98  interaction_matrices[j_ * order][0][VZ] =
99  (j + 3 * delta) * (A * m.get(1, j_) + B * m.get(0, jp1_) + C * m.get(0, j_)) - delta * C * m.get(0, j_);
100 
101  interaction_matrices[j_ * order][0][WX] = (j + 3 * delta) * m.get(0, jp1_) + j * m.get(0, jm1_);
102  interaction_matrices[j_ * order][0][WY] = -(j + 3 * delta) * m.get(1, j_);
103  interaction_matrices[j_ * order][0][WZ] = -j * m.get(1, jm1_);
104  }
105 
106  // int j=0;
107  for (int i = 1; i < (int)order - 1; i++) {
108  unsigned int i_ = (unsigned int)i;
109  unsigned int im1_ = i_ - 1;
110  unsigned int ip1_ = i_ + 1;
111 
112  interaction_matrices[i_][0][VX] =
113  -i * (A * m.get(i_, 0) + B * m.get(im1_, 1) + C * m.get(im1_, 0)) - delta * A * m.get(i_, 0);
114  interaction_matrices[i_][0][VY] = -delta * B * m.get(i_, 0);
115  interaction_matrices[i_][0][VZ] =
116  (i + 3 * delta) * (A * m.get(ip1_, 0) + B * m.get(i_, 1) + C * m.get(i_, 0)) - delta * C * m.get(i_, 0);
117 
118  interaction_matrices[i_][0][WX] = (i + 3 * delta) * m.get(i_, 1);
119  interaction_matrices[i_][0][WY] = -(i + 3 * delta) * m.get(ip1_, 0) - i * m.get(im1_, 0);
120  interaction_matrices[i_][0][WZ] = i * m.get(im1_, 1);
121  }
122 
123  for (int j = 1; j < (int)order - 1; j++) {
124  unsigned int j_ = (unsigned int)j;
125  unsigned int jm1_ = j_ - 1;
126  unsigned int jp1_ = j_ + 1;
127 
128  for (int i = 1; i < (int)order - j - 1; i++) {
129  unsigned int i_ = (unsigned int)i;
130  unsigned int im1_ = i_ - 1;
131  unsigned int ip1_ = i_ + 1;
132 
133  interaction_matrices[j_ * order + i_][0][VX] =
134  -i * (A * m.get(i_, j_) + B * m.get(im1_, jp1_) + C * m.get(im1_, j_)) - delta * A * m.get(i_, j_);
135  interaction_matrices[j_ * order + i_][0][VY] =
136  -j * (A * m.get(ip1_, jm1_) + B * m.get(i_, j_) + C * m.get(i_, jm1_)) - delta * B * m.get(i_, j_);
137  interaction_matrices[j_ * order + i_][0][VZ] =
138  (i + j + 3 * delta) * (A * m.get(ip1_, j_) + B * m.get(i_, jp1_) + C * m.get(i_, j_)) -
139  delta * C * m.get(i_, j_);
140 
141  interaction_matrices[j_ * order + i_][0][WX] = (i + j + 3 * delta) * m.get(i_, jp1_) + j * m.get(i_, jm1_);
142  interaction_matrices[j_ * order + i_][0][WY] = -(i + j + 3 * delta) * m.get(ip1_, j_) - i * m.get(im1_, j_);
143  interaction_matrices[j_ * order + i_][0][WZ] = i * m.get(im1_, jp1_) - j * m.get(ip1_, jm1_);
144  }
145  }
146 }
147 
154 vpMatrix vpFeatureMomentBasic::interaction(unsigned int select_one, unsigned int select_two) const
155 {
156  if (select_one + select_two > moment->getObject().getOrder())
157  throw vpException(vpException::badValue, "The requested value has not "
158  "been computed, you should "
159  "specify a higher order.");
160  return interaction_matrices[select_two * order + select_one];
161 }
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
void compute_interaction() vp_override
vpFeatureMomentBasic(vpMomentDatabase &moments, double A, double B, double C, vpFeatureMomentDatabase *featureMoments=nullptr)
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
This class allows to register all feature moments (implemented in vpFeatureMoment....
This class defines shared system methods/attributes for 2D moment features but no functional code....
std::vector< vpMatrix > interaction_matrices
const vpMoment * moment
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:151
This class allows to register all vpMoments so they can access each other according to their dependen...
Class for generic objects.
unsigned int getOrder() const
const std::vector< double > & get() const
vpObjectType getType() const
const vpMomentObject & getObject() const
Definition: vpMoment.h:145