Visual Servoing Platform  version 3.0.0
vpFeatureMomentBasic.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 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 
39 
40 #include <visp3/core/vpMomentObject.h>
41 #include <visp3/visual_features/vpFeatureMomentBasic.h>
42 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
43 #include <vector>
44 #include <limits>
53 vpFeatureMomentBasic::vpFeatureMomentBasic(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments) :
54  vpFeatureMoment(data_base,A_,B_,C_,featureMoments), order(0)
55 {
56 }
57 
63  int delta;
64  const vpMomentObject& m = moment->getObject();
65  order = m.getOrder()+1;
67  for(std::vector< vpMatrix >::iterator i=interaction_matrices.begin();i!=interaction_matrices.end();i++)
68  i->resize(1,6);
70  delta=0;
71  } else {
72  delta=1;
73  }
74 
75  int VX = 0;
76  int VY = 1;
77  int VZ = 2;
78  int WX = 3;
79  int WY = 4;
80  int WZ = 5;
81 
82  //i=0;j=0
83  interaction_matrices[0][0][VX] = -delta*A*m.get(0, 0);
84  interaction_matrices[0][0][VY] = -delta*B*m.get(0, 0);
85  interaction_matrices[0][0][VZ] = 3*delta*(A*m.get(1, 0)+B*m.get(0, 1)+C*m.get(0, 0))-delta*C*m.get(0, 0);
86 
87  interaction_matrices[0][0][WX] = 3*delta*m.get(0, 1);
88  interaction_matrices[0][0][WY] = -3*delta*m.get(1, 0);
89  interaction_matrices[0][0][WZ] = 0;
90 
91  // int i=0;
92  for(int j=1;j<(int)order-1;j++){
93  unsigned int j_ = (unsigned int) j;
94  unsigned int jm1_ = j_ - 1;
95  unsigned int jp1_ = j_ + 1;
96 
97  interaction_matrices[j_*order][0][VX] = -delta*A*m.get(0, j_);
98  interaction_matrices[j_*order][0][VY] = -j*(A*m.get(1,jm1_)+B*m.get(0,j_)+C*m.get(0,jm1_))-delta*B*m.get(0,j_);
99  interaction_matrices[j_*order][0][VZ] = (j+3*delta)*(A*m.get(1,j_)+B*m.get(0,jp1_)+C*m.get(0,j_))-delta*C*m.get(0,j_);
100 
101  interaction_matrices[j_*order][0][WX] = (j+3*delta)*m.get(0,jp1_)+j*m.get(0,jm1_);
102  interaction_matrices[j_*order][0][WY] = -(j+3*delta)*m.get(1,j_);
103  interaction_matrices[j_*order][0][WZ] = -j*m.get(1,jm1_);
104  }
105 
106  //int j=0;
107  for(int i=1;i<(int)order-1;i++){
108  unsigned int i_ = (unsigned int) i;
109  unsigned int im1_ = i_ - 1;
110  unsigned int ip1_ = i_ + 1;
111 
112  interaction_matrices[i_][0][VX] = -i*(A*m.get(i_, 0)+B*m.get(im1_, 1)+C*m.get(im1_, 0))-delta*A*m.get(i_, 0);
113  interaction_matrices[i_][0][VY] = -delta*B*m.get(i_, 0);
114  interaction_matrices[i_][0][VZ] = (i+3*delta)*(A*m.get(ip1_, 0)+B*m.get(i_, 1)+C*m.get(i_, 0))-delta*C*m.get(i_, 0);
115 
116  interaction_matrices[i_][0][WX] = (i+3*delta)*m.get(i_, 1);
117  interaction_matrices[i_][0][WY] = -(i+3*delta)*m.get(ip1_, 0)-i*m.get(im1_, 0);
118  interaction_matrices[i_][0][WZ] = i*m.get(im1_, 1);
119  }
120 
121  for(int j=1; j<(int)order-1; j++){
122  unsigned int j_ = (unsigned int) j;
123  unsigned int jm1_ = j_ - 1;
124  unsigned int jp1_ = j_ + 1;
125 
126  for(int i=1; i<(int)order-j-1; i++){
127  unsigned int i_ = (unsigned int) i;
128  unsigned int im1_ = i_ - 1;
129  unsigned int ip1_ = i_ + 1;
130 
131  interaction_matrices[j_*order+i_][0][VX] = -i*(A*m.get(i_, j_)+B*m.get(im1_, jp1_)+C*m.get(im1_,j_))-delta*A*m.get(i_, j_);
132  interaction_matrices[j_*order+i_][0][VY] = -j*(A*m.get(ip1_, jm1_)+B*m.get(i_, j_)+C*m.get(i_,jm1_))-delta*B*m.get(i_, j_);
133  interaction_matrices[j_*order+i_][0][VZ] = (i+j+3*delta)*(A*m.get(ip1_, j_)+B*m.get(i_,jp1_)+C*m.get(i_, j_))-delta*C*m.get(i_,j_);
134 
135  interaction_matrices[j_*order+i_][0][WX] = (i+j+3*delta)*m.get(i_, jp1_)+j*m.get(i_, jm1_);
136  interaction_matrices[j_*order+i_][0][WY] = -(i+j+3*delta)*m.get(ip1_, j_)-i*m.get(im1_, j_);
137  interaction_matrices[j_*order+i_][0][WZ] = i*m.get(im1_,jp1_)-j*m.get(ip1_, jm1_);
138  }
139  }
140 }
141 
148 vpMatrix vpFeatureMomentBasic::interaction (unsigned int select_one,unsigned int select_two) const {
149  if(select_one+select_two>moment->getObject().getOrder())
150  throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");
151  return interaction_matrices[select_two*order+select_one];
152 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
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
This class allows to register all vpMoments so they can access each other according to their dependen...
This class defines shared system methods/attributes for 2D moment features but no functional code...
vpObjectType getType() const
vpFeatureMomentBasic(vpMomentDatabase &moments, double A, double B, double C, vpFeatureMomentDatabase *featureMoments=NULL)
const vpMomentObject & getObject() const
Definition: vpMoment.h:143
This class allows to register all feature moments (implemented in vpFeatureMoment... classes) so they can access each other according to their dependencies.
unsigned int getOrder() const