Visual Servoing Platform  version 3.2.1 under development (2019-10-21) under development (2019-10-21)
vpFeatureMomentGravityCenter.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 <visp3/core/vpConfig.h>
40 
41 #ifdef VISP_MOMENTS_COMBINE_MATRICES
42 
43 #include <limits>
44 #include <vector>
45 
46 #include <visp3/core/vpMomentObject.h>
47 #include <visp3/visual_features/vpFeatureMomentBasic.h>
48 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
49 #include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
50 
60 {
61  bool found_featuremoment_basic;
62 
63  vpFeatureMomentBasic &featureMomentBasic = (static_cast<vpFeatureMomentBasic &>(
64  featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic)));
65  const vpMomentObject &momentObject = moment->getObject();
66 
67  if (!found_featuremoment_basic)
68  throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found");
69 
70  interaction_matrices[0].resize(1, 6);
71  interaction_matrices[1].resize(1, 6);
72 
74  featureMomentBasic.interaction(1, 0) / momentObject.get(0, 0) -
75  momentObject.get(1, 0) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
77  featureMomentBasic.interaction(0, 1) / momentObject.get(0, 0) -
78  momentObject.get(0, 1) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
79 }
80 
81 #else
82 
83 #include <limits>
84 #include <vector>
85 
86 #include <visp3/core/vpMomentCentered.h>
87 #include <visp3/core/vpMomentDatabase.h>
88 #include <visp3/core/vpMomentGravityCenter.h>
89 #include <visp3/core/vpMomentObject.h>
90 #include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
91 
102 {
103  bool found_moment_centered;
104  bool found_moment_gravity;
105 
106  const vpMomentCentered &momentCentered =
107  (static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered)));
108  const vpMomentGravityCenter &momentGravity =
109  static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
110 
111  const vpMomentObject &momentObject = moment->getObject();
112 
113  if (!found_moment_centered)
114  throw vpException(vpException::notInitialized, "vpMomentCentered not found");
115  if (!found_moment_gravity)
116  throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
117 
118  interaction_matrices[0].resize(1, 6);
119  interaction_matrices[1].resize(1, 6);
120  int epsilon;
121  if (momentObject.getType() == vpMomentObject::DISCRETE) {
122  epsilon = 1;
123  } else {
124  epsilon = 4;
125  }
126  double n11 = momentCentered.get(1, 1) / momentObject.get(0, 0);
127  double n20 = momentCentered.get(2, 0) / momentObject.get(0, 0);
128  double n02 = momentCentered.get(0, 2) / momentObject.get(0, 0);
129  double Xg = momentGravity.getXg();
130  double Yg = momentGravity.getYg();
131  double Zg_inv = A * Xg + B * Yg + C;
132  double Xgvz = Xg * Zg_inv + A * epsilon * n20 + B * epsilon * n11;
133  double Ygvz = Yg * Zg_inv + A * epsilon * n11 + B * epsilon * n02;
134  double Xgwx = Xg * Yg + epsilon * n11;
135  double Ygwy = -Xgwx;
136  double Xgwy = -(1 + Xg * Xg + epsilon * n20);
137  double Ygwx = 1 + Yg * Yg + epsilon * n02;
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] = -Zg_inv;
147  interaction_matrices[0][0][VY] = 0;
148  interaction_matrices[0][0][VZ] = Xgvz;
149 
150  interaction_matrices[0][0][WX] = Xgwx;
151  interaction_matrices[0][0][WY] = Xgwy;
152  interaction_matrices[0][0][WZ] = Yg;
153 
154  interaction_matrices[1][0][VX] = 0;
155  interaction_matrices[1][0][VY] = -Zg_inv;
156  interaction_matrices[1][0][VZ] = Ygvz;
157 
158  interaction_matrices[1][0][WX] = Ygwx;
159  interaction_matrices[1][0][WY] = Ygwy;
160  interaction_matrices[1][0][WZ] = -Xg;
161 }
162 
163 #endif
Functionality computation for basic moment feature. Computes the interaction matrix associated with v...
double get(unsigned int i, unsigned int j) const
const vpMoment * moment
const vpMoment & get(const char *type, bool &found) const
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class for generic objects.
std::vector< vpMatrix > interaction_matrices
const vpMomentObject & getObject() const
Definition: vpMoment.h:150
vpMomentDatabase & moments
const std::vector< double > & get() const
This class defines the double-indexed centered moment descriptor .
vpObjectType getType() const
Class describing 2D gravity center moment.
vpFeatureMomentDatabase * featureMomentsDataBase
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
vpFeatureMoment & get(const char *type, bool &found)