Visual Servoing Platform version 3.5.0
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 u11 = momentCentered.get(1,1);
74 double u20_u02 = momentCentered.get(2,0)-momentCentered.get(0,2);
75 double dinv = 1 / (4*u11*u11 + u20_u02*u20_u02);
76 interaction_matrices[0].resize(1, 6);
77 interaction_matrices[0] = (u20_u02*dinv) * featureMomentCentered.interaction(1,1) +
78 (u11*dinv) * (featureMomentCentered.interaction(0,2)-featureMomentCentered.interaction(2,0));
79}
80
81#else // #ifdef VISP_MOMENTS_COMBINE_MATRICES
82
91{
92 bool found_moment_centered;
93 bool found_moment_gravity;
94
95 const vpMomentCentered &momentCentered =
96 static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered));
97 const vpMomentGravityCenter &momentGravity =
98 static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
99 const vpMomentObject &momentObject = moment->getObject();
100
101 if (!found_moment_centered)
102 throw vpException(vpException::notInitialized, "vpMomentCentered not found");
103 if (!found_moment_gravity)
104 throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
105
106 double mu11 = momentCentered.get(1, 1);
107 double mu20 = momentCentered.get(2, 0);
108 double mu02 = momentCentered.get(0, 2);
109 double mu12 = momentCentered.get(1, 2);
110 double mu21 = momentCentered.get(2, 1);
111 double mu03 = momentCentered.get(0, 3);
112 double mu30 = momentCentered.get(3, 0);
113
114 double Xg = momentGravity.getXg();
115 double Yg = momentGravity.getYg();
116
117 double Avx, Avy, Avz, Awx, Awy;
118 double beta = (momentObject.getType() == vpMomentObject::DISCRETE) ? 2 : 5;
119
120 double d = (mu20 - mu02) * (mu20 - mu02) + 4 * mu11 * mu11;
121 double DA = mu20 + mu02;
122 double DA_2 = DA * DA;
123 double mu11_2 = mu11 * mu11;
124
125 Avx = mu11 * DA * A / d + (DA * mu02 + (0.5) * d - (0.5) * DA_2) * B / d;
126 Avy = (DA * mu02 - (0.5) * d - (.5) * DA_2) * A / d - B * mu11 * DA / d;
127
128 Awx = (beta * (mu12 * (mu20 - mu02) + mu11 * (mu03 - mu21)) + Xg * (mu02 * (mu20 - mu02) - 2 * mu11_2) +
129 Yg * mu11 * (mu20 + mu02)) /
130 d;
131 Awy = (beta * (mu21 * (mu02 - mu20) + mu11 * (mu30 - mu12)) + Xg * mu11 * (mu20 + mu02) +
132 Yg * (mu20 * (mu02 - mu20) - 2 * mu11_2)) /
133 d;
134
135 Avz = B * Awx - A * Awy;
136 interaction_matrices.resize(1);
137 interaction_matrices[0].resize(1, 6);
138
139 int VX = 0;
140 int VY = 1;
141 int VZ = 2;
142 int WX = 3;
143 int WY = 4;
144 int WZ = 5;
145
146 interaction_matrices[0][0][VX] = Avx;
147 interaction_matrices[0][0][VY] = Avy;
148 interaction_matrices[0][0][VZ] = Avz;
149
150 interaction_matrices[0][0][WX] = Awx;
151 interaction_matrices[0][0][WY] = Awy;
152 interaction_matrices[0][0][WZ] = -1.;
153}
154
155#endif // #ifdef VISP_MOMENTS_COMBINE_MATRICES
156
157vpColVector vpFeatureMomentAlpha::error(const vpBasicFeature &s_star, const unsigned int /* select */)
158{
159 vpColVector e(0);
160 double err = s[0] - s_star[0];
161
162 if (err < -M_PI)
163 err += 2 * M_PI;
164 if (err > M_PI)
165 err -= 2 * M_PI;
166
167 vpColVector ecv(1);
168 ecv[0] = err;
169 e = vpColVector::stack(e, ecv);
170
171 return e;
172}
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:131
void stack(double d)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
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 char *type, 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 char *type, bool &found) const
Class describing 2D gravity center moment.
Class for generic objects.
vpObjectType getType() const
const vpMomentObject & getObject() const
Definition: vpMoment.h:150