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