Visual Servoing Platform  version 3.2.1 under development (2019-07-18)
vpFeatureMomentAlpha.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 alpha moment features.
33  *
34  * Authors:
35  * Filip Novotny
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpMomentCentered.h>
40 #include <visp3/core/vpMomentGravityCenter.h>
41 #include <visp3/core/vpMomentObject.h>
42 #include <visp3/visual_features/vpFeatureMomentAlpha.h>
43 #include <visp3/visual_features/vpFeatureMomentCentered.h>
44 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
45 
46 #include <limits>
47 #include <vector>
48 
49 #ifdef VISP_MOMENTS_COMBINE_MATRICES
50 
59 {
60  bool found_moment_centered;
61  bool found_FeatureMoment_centered;
62 
63  const vpMomentCentered &momentCentered =
64  (static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered)));
65  vpFeatureMomentCentered &featureMomentCentered = (static_cast<vpFeatureMomentCentered &>(
66  featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered)));
67 
68  if (!found_moment_centered)
69  throw vpException(vpException::notInitialized, "vpMomentCentered not found");
70  if (!found_FeatureMoment_centered)
71  throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
72 
73  double multiplier =
74  -1. /
75  (momentCentered.get(2, 0) * momentCentered.get(2, 0) - 2 * momentCentered.get(0, 2) * momentCentered.get(2, 0) +
76  4 * momentCentered.get(1, 1) * momentCentered.get(1, 1) + momentCentered.get(0, 2) * momentCentered.get(0, 2));
77 
78  interaction_matrices[0].resize(1, 6);
80  multiplier * (momentCentered.get(1, 1) * featureMomentCentered.interaction(2, 0) +
81  (momentCentered.get(0, 2) - momentCentered.get(2, 0)) * featureMomentCentered.interaction(1, 1) -
82  momentCentered.get(1, 1) * featureMomentCentered.interaction(0, 2));
83 }
84 
85 #else
86 
95 {
96  bool found_moment_centered;
97  bool found_moment_gravity;
98 
99  const vpMomentCentered &momentCentered =
100  static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered));
101  const vpMomentGravityCenter &momentGravity =
102  static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
103  const vpMomentObject &momentObject = moment->getObject();
104 
105  if (!found_moment_centered)
106  throw vpException(vpException::notInitialized, "vpMomentCentered not found");
107  if (!found_moment_gravity)
108  throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
109 
110  double mu11 = momentCentered.get(1, 1);
111  double mu20 = momentCentered.get(2, 0);
112  double mu02 = momentCentered.get(0, 2);
113  double mu12 = momentCentered.get(1, 2);
114  double mu21 = momentCentered.get(2, 1);
115  double mu03 = momentCentered.get(0, 3);
116  double mu30 = momentCentered.get(3, 0);
117 
118  double Xg = momentGravity.getXg();
119  double Yg = momentGravity.getYg();
120 
121  double Avx, Avy, Avz, Awx, Awy;
122  double beta, gamma;
123 
124  if (momentObject.getType() == vpMomentObject::DISCRETE) {
125  beta = 4;
126  gamma = 2;
127  } else {
128  beta = 5;
129  gamma = 1;
130  }
131 
132  double d = (mu20 - mu02) * (mu20 - mu02) + 4 * mu11 * mu11;
133  double DA = mu20 + mu02;
134  double DA_2 = DA * DA;
135  double mu11_2 = mu11 * mu11;
136 
137  Avx = mu11 * DA * A / d + (DA * mu02 + (0.5) * d - (0.5) * DA_2) * B / d;
138  Avy = (DA * mu02 - (0.5) * d - (.5) * DA_2) * A / d - B * mu11 * DA / d;
139 
140  Awx = (beta * (mu12 * (mu20 - mu02) + mu11 * (mu03 - mu21)) + gamma * Xg * (mu02 * (mu20 - mu02) - 2 * mu11_2) +
141  gamma * Yg * mu11 * (mu20 + mu02)) /
142  d;
143  Awy = (beta * (mu21 * (mu02 - mu20) + mu11 * (mu30 - mu12)) + gamma * Xg * mu11 * (mu20 + mu02) +
144  gamma * Yg * (mu20 * (mu02 - mu20) - 2 * mu11_2)) /
145  d;
146 
147  Avz = B * Awx - A * Awy;
148  interaction_matrices.resize(1);
149  interaction_matrices[0].resize(1, 6);
150 
151  int VX = 0;
152  int VY = 1;
153  int VZ = 2;
154  int WX = 3;
155  int WY = 4;
156  int WZ = 5;
157 
158  interaction_matrices[0][0][VX] = Avx;
159  interaction_matrices[0][0][VY] = Avy;
160  interaction_matrices[0][0][VZ] = Avz;
161 
162  interaction_matrices[0][0][WX] = Awx;
163  interaction_matrices[0][0][WY] = Awy;
164  interaction_matrices[0][0][WZ] = -1.;
165 }
166 
167 vpColVector vpFeatureMomentAlpha::error(const vpBasicFeature &s_star, const unsigned int /* select */)
168 {
169  vpColVector e(0);
170  double err = s[0] - s_star[0];
171 
172  if (err < -M_PI)
173  err += 2 * M_PI;
174  if (err > M_PI)
175  err -= 2 * M_PI;
176 
177  vpColVector ecv(1);
178  ecv[0] = err;
179  e = vpColVector::stack(e, ecv);
180 
181  return e;
182 }
183 #endif
const vpMoment * moment
Functionality computation for centered moment feature. Computes the interaction matrix associated wit...
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class for generic objects.
std::vector< vpMatrix > interaction_matrices
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
class that defines what is a visual feature
const vpMoment & get(const char *type, bool &found) const
vpMomentDatabase & moments
This class defines the double-indexed centered moment descriptor .
double get(unsigned int i, unsigned int j) const
Class describing 2D gravity center moment.
vpObjectType getType() const
vpFeatureMomentDatabase * featureMomentsDataBase
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
const vpMomentObject & getObject() const
Definition: vpMoment.h:150
vpFeatureMoment & get(const char *type, bool &found)
vpColVector s
State of the visual feature.