Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpFeatureMomentAlpha.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 alpha moment features.
32  *
33  * Authors:
34  * Filip Novotny
35  *
36  *****************************************************************************/
37 
38 #include <visp3/core/vpMomentObject.h>
39 #include <visp3/core/vpMomentGravityCenter.h>
40 #include <visp3/visual_features/vpFeatureMomentAlpha.h>
41 #include <visp3/core/vpMomentCentered.h>
42 #include <visp3/visual_features/vpFeatureMomentCentered.h>
43 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
44 
45 #include <vector>
46 #include <limits>
47 
48 #ifdef VISP_MOMENTS_COMBINE_MATRICES
49 
58  bool found_moment_centered;
59  bool found_FeatureMoment_centered;
60 
61  const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
62  vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
63 
64  if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
65  if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
66 
67  double multiplier = -1./(momentCentered.get(2,0)*momentCentered.get(2,0)-2*momentCentered.get(0,2)*momentCentered.get(2,0)+4*momentCentered.get(1,1)*momentCentered.get(1,1)+momentCentered.get(0,2)*momentCentered.get(0,2));
68 
69  interaction_matrices[0].resize(1,6);
70  interaction_matrices[0] = multiplier*(momentCentered.get(1,1)*featureMomentCentered.interaction(2,0)+(momentCentered.get(0,2)-momentCentered.get(2,0))*featureMomentCentered.interaction(1,1)-momentCentered.get(1,1)*featureMomentCentered.interaction(0,2));
71 }
72 
73 #else
74 
83  bool found_moment_centered;
84  bool found_moment_gravity;
85 
86  const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
87  const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
88  const vpMomentObject& momentObject = moment->getObject();
89 
90 
91  if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
92  if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
93 
94  double mu11 = momentCentered.get(1,1);
95  double mu20 = momentCentered.get(2,0);
96  double mu02 = momentCentered.get(0,2);
97  double mu12 = momentCentered.get(1,2);
98  double mu21 = momentCentered.get(2,1);
99  double mu03 = momentCentered.get(0,3);
100  double mu30 = momentCentered.get(3,0);
101 
102  double Xg = momentGravity.getXg();
103  double Yg = momentGravity.getYg();
104 
105  double Avx,Avy,Avz,Awx,Awy;
106  double beta,gamma;
107 
108  if (momentObject.getType()==vpMomentObject::DISCRETE){
109  beta = 4;
110  gamma = 2;
111  } else {
112  beta = 5;
113  gamma = 1;
114  }
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))+gamma*Xg*(mu02*(mu20-mu02)-2*mu11_2)+gamma*Yg*mu11*(mu20+mu02))/d;
125  Awy = (beta*(mu21*(mu02-mu20)+mu11*(mu30-mu12))+gamma*Xg*mu11*(mu20+mu02)+gamma*Yg*(mu20*(mu02-mu20)-2*mu11_2))/d;
126 
127  Avz = B*Awx-A*Awy;
128  interaction_matrices.resize(1);
129  interaction_matrices[0].resize(1,6);
130 
131  int VX = 0;
132  int VY = 1;
133  int VZ = 2;
134  int WX = 3;
135  int WY = 4;
136  int WZ = 5;
137 
138  interaction_matrices[0][0][VX] = Avx;
139  interaction_matrices[0][0][VY] = Avy;
140  interaction_matrices[0][0][VZ] = Avz;
141 
142  interaction_matrices[0][0][WX] = Awx;
143  interaction_matrices[0][0][WY] = Awy;
144  interaction_matrices[0][0][WZ] = -1.;
145 }
146 
147 vpColVector vpFeatureMomentAlpha::error (const vpBasicFeature &s_star, const unsigned int /* select */){
148  vpColVector e(0) ;
149  double err = s[0] - s_star[0] ;
150 
151  if (err < -M_PI) err += 2*M_PI ;
152  if (err > M_PI) err -= 2*M_PI ;
153 
154  vpColVector ecv(1) ;
155  ecv[0] = err ;
156  e = vpColVector::stack(e,ecv) ;
157 
158  return e;
159 }
160 #endif
void stack(const double &d)
const vpMoment * moment
Functionality computation for centered moment feature. Computes the interaction matrix associated wit...
error that can be emited by ViSP classes.
Definition: vpException.h:73
Class for generic objects.
std::vector< vpMatrix > interaction_matrices
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
Compute the error between two visual features from a subset of the possible features.
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
class that defines what is a visual feature
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
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
const vpMomentObject & getObject() const
Definition: vpMoment.h:145
vpFeatureMoment & get(const char *type, bool &found)
vpColVector s
State of the visual feature.