Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpFeatureMomentGravityCenter.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Authors:
34  * Filip Novotny
35  *
36  *****************************************************************************/
37 
38 #include <visp3/core/vpConfig.h>
39 
40 #ifdef VISP_MOMENTS_COMBINE_MATRICES
41 
42 #include <vector>
43 #include <limits>
44 
45 #include <visp3/core/vpMomentObject.h>
46 #include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
47 #include <visp3/visual_features/vpFeatureMomentBasic.h>
48 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
49 
50 
60  bool found_featuremoment_basic;
61 
62  vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
63  const vpMomentObject& momentObject = moment->getObject();
64 
65  if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
66 
67 
68  interaction_matrices[0].resize(1,6);
69  interaction_matrices[1].resize(1,6);
70 
71  interaction_matrices[0] = featureMomentBasic.interaction(1, 0) / momentObject.get(0, 0) - momentObject.get(1, 0) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
72  interaction_matrices[1] = featureMomentBasic.interaction(0, 1) / momentObject.get(0, 0) - momentObject.get(0, 1) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
73 
74 
75 }
76 
77 #else
78 
79 #include <vector>
80 #include <limits>
81 
82 #include <visp3/core/vpMomentObject.h>
83 #include <visp3/core/vpMomentDatabase.h>
84 #include <visp3/core/vpMomentGravityCenter.h>
85 #include <visp3/core/vpMomentCentered.h>
86 #include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
87 #include <visp3/core/vpMomentObject.h>
88 
89 
100  bool found_moment_centered;
101  bool found_moment_gravity;
102 
103  const vpMomentCentered& momentCentered= (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
104  const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
105 
106 
107  const vpMomentObject& momentObject = moment->getObject();
108 
109  if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
110  if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
111 
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  } else {
119  epsilon=4;
120  }
121  double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
122  double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
123  double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
124  double Xg = momentGravity.getXg();
125  double Yg = momentGravity.getYg();
126  double Zg_inv = A*Xg+B*Yg+C;
127  double Xgvz = Xg*Zg_inv+A*epsilon*n20+B*epsilon*n11;
128  double Ygvz = Yg*Zg_inv+A*epsilon*n11+B*epsilon*n02;
129  double Xgwx = Xg*Yg+epsilon*n11;
130  double Ygwy = -Xgwx;
131  double Xgwy = -(1+Xg*Xg+epsilon*n20);
132  double Ygwx = 1+Yg*Yg+epsilon*n02;
133 
134  int VX = 0;
135  int VY = 1;
136  int VZ = 2;
137  int WX = 3;
138  int WY = 4;
139  int WZ = 5;
140 
141  interaction_matrices[0][0][VX] = -Zg_inv;
142  interaction_matrices[0][0][VY] = 0;
143  interaction_matrices[0][0][VZ] = Xgvz;
144 
145  interaction_matrices[0][0][WX] = Xgwx;
146  interaction_matrices[0][0][WY] = Xgwy;
147  interaction_matrices[0][0][WZ] = Yg;
148 
149  interaction_matrices[1][0][VX] = 0;
150  interaction_matrices[1][0][VY] = -Zg_inv;
151  interaction_matrices[1][0][VZ] = Ygvz;
152 
153  interaction_matrices[1][0][WX] = Ygwx;
154  interaction_matrices[1][0][WY] = Ygwy;
155  interaction_matrices[1][0][WZ] = -Xg;
156 
157 }
158 
159 #endif
Functionality computation for basic moment feature. Computes the interaction matrix associated with v...
const vpMoment * moment
error that can be emited by ViSP classes.
Definition: vpException.h:73
Class for generic objects.
const std::vector< double > & get() const
std::vector< vpMatrix > interaction_matrices
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
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
const vpMomentObject & getObject() const
Definition: vpMoment.h:145
vpFeatureMoment & get(const char *type, bool &found)