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