Visual Servoing Platform  version 3.6.1 under development (2024-06-23)
vpFeatureMomentGravityCenter.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 <visp3/core/vpConfig.h>
35 
36 #ifdef VISP_MOMENTS_COMBINE_MATRICES
37 
38 #include <vector>
39 
40 #include <visp3/core/vpMomentObject.h>
41 #include <visp3/visual_features/vpFeatureMomentBasic.h>
42 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
43 #include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
44 
55 {
56  bool found_featuremoment_basic;
57 
58  vpFeatureMomentBasic &featureMomentBasic = (static_cast<vpFeatureMomentBasic &>(
59  featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic)));
60  const vpMomentObject &momentObject = moment->getObject();
61 
62  if (!found_featuremoment_basic)
63  throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found");
64 
65  interaction_matrices[0].resize(1, 6);
66  interaction_matrices[1].resize(1, 6);
67 
69  featureMomentBasic.interaction(1, 0) / momentObject.get(0, 0) -
70  momentObject.get(1, 0) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
72  featureMomentBasic.interaction(0, 1) / momentObject.get(0, 0) -
73  momentObject.get(0, 1) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
74 }
76 #else
77 
78 #include <vector>
79 
80 #include <visp3/core/vpMomentCentered.h>
81 #include <visp3/core/vpMomentDatabase.h>
82 #include <visp3/core/vpMomentGravityCenter.h>
83 #include <visp3/core/vpMomentObject.h>
84 #include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
85 
97 {
98  bool found_moment_centered;
99  bool found_moment_gravity;
100 
101  const vpMomentCentered &momentCentered =
102  (static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered)));
103  const vpMomentGravityCenter &momentGravity =
104  static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
105 
106  const vpMomentObject &momentObject = moment->getObject();
107 
108  if (!found_moment_centered)
109  throw vpException(vpException::notInitialized, "vpMomentCentered not found");
110  if (!found_moment_gravity)
111  throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
112 
113  interaction_matrices[0].resize(1, 6);
114  interaction_matrices[1].resize(1, 6);
115  int epsilon;
116  if (momentObject.getType() == vpMomentObject::DISCRETE) {
117  epsilon = 1;
118  }
119  else {
120  epsilon = 4;
121  }
122  double n11 = momentCentered.get(1, 1) / momentObject.get(0, 0);
123  double n20 = momentCentered.get(2, 0) / momentObject.get(0, 0);
124  double n02 = momentCentered.get(0, 2) / momentObject.get(0, 0);
125  double Xg = momentGravity.getXg();
126  double Yg = momentGravity.getYg();
127  double Zg_inv = A * Xg + B * Yg + C;
128  double Xgvz = Xg * Zg_inv + A * epsilon * n20 + B * epsilon * n11;
129  double Ygvz = Yg * Zg_inv + A * epsilon * n11 + B * epsilon * n02;
130  double Xgwx = Xg * Yg + epsilon * n11;
131  double Ygwy = -Xgwx;
132  double Xgwy = -(1 + Xg * Xg + epsilon * n20);
133  double Ygwx = 1 + Yg * Yg + epsilon * n02;
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] = -Zg_inv;
143  interaction_matrices[0][0][VY] = 0;
144  interaction_matrices[0][0][VZ] = Xgvz;
145 
146  interaction_matrices[0][0][WX] = Xgwx;
147  interaction_matrices[0][0][WY] = Xgwy;
148  interaction_matrices[0][0][WZ] = Yg;
149 
150  interaction_matrices[1][0][VX] = 0;
151  interaction_matrices[1][0][VY] = -Zg_inv;
152  interaction_matrices[1][0][VZ] = Ygvz;
153 
154  interaction_matrices[1][0][WX] = Ygwx;
155  interaction_matrices[1][0][WY] = Ygwy;
156  interaction_matrices[1][0][WZ] = -Xg;
157 }
159 #endif
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:74
Functionality computation for basic moment feature. Computes the interaction matrix associated with v...
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.
const std::vector< double > & get() const
vpObjectType getType() const
const vpMomentObject & getObject() const
Definition: vpMoment.h:145