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 * Generic feature (used to create new feature not implemented in ViSP). 33 * 34 * Authors: 35 * Eric Marchand 36 * 37 *****************************************************************************/ 38 39 #ifndef vpGenericFeature_hh 40 #define vpGenericFeature_hh 41 42 /*! 43 \file vpGenericFeature.h 44 \brief class that defines what is a generic feature (used to create new 45 feature not implemented in ViSP2 46 */ 47 48 #include <visp3/core/vpMatrix.h> 49 #include <visp3/core/vpRGBa.h> 50 #include <visp3/visual_features/vpBasicFeature.h> 51 52 #include <math.h> 53 54 /*! 55 \class vpGenericFeature 56 \ingroup group_core_features 57 58 \brief Class that enables to define a feature or a set of features which are 59 not implemented in ViSP as a specific class. It is indeed possible to create 60 its own features, to use the corresponding interaction matrix, and to compute 61 an error between the current and the desired feature. Moreover the created 62 features can be mixed with features already implemented. 63 64 The following example shows how to use the vpGenericFeature class to create 65 and use the feature \f$ log(Z) \f$ where Z corresponds to the depth of a point 66 whose 2D coordinates in the camera frame are \f$ x \f$ and \f$ y \f$. The 67 interaction matrix corresponding to this feature is \f[ L = 68 \left[\begin{array}{cccccc} 0 & 0 & -1/Z & -y & x & 0 \end{array}\right]\f]. 69 \code 70 #include <visp3/core/vpGenericFeature.h> 71 #include <visp3/vs/vpServo.h> 72 73 int main() 74 { 75 vpServo task; // Visual servoing task 76 77 //First we have to define the desired feature log(Z*) corresponding to the desired point. 78 double xd = 0; //The x coordinate of the desired point. 79 double yd = 0; //The y coordinate of the desired point. 80 double Zd = 1; //The depth of the desired point. 81 vpGenericFeature logZd(1); //The dimension of the feature is 1. 82 logZd.set_s( log(Zd) ); 83 84 //Then we have to define the current feature log(Z) corresponding to the current point. 85 double x = 1; //The x coordinate of the current point. 86 double y = 1; //The y coordinate of the current point. 87 double Z = 2; //The depth of the current point. 88 vpGenericFeature logZ(1); //The dimension of the feature is 1. 89 logZ.set_s( log(Z) ); 90 91 // Set eye-in-hand control law. 92 // The computed velocities will be expressed in the camera frame 93 task.setServo(vpServo::EYEINHAND_CAMERA); 94 // Interaction matrix is computed with the current visual features sd 95 task.setInteractionMatrixType(vpServo::CURRENT); 96 97 // Add the point feature to the task 98 task.addFeature(logZ, logZd); 99 100 // Control loop 101 for ( ; ; ) { 102 // The new parameters x, y and Z must be computed here. 103 104 // Update the current point visual feature 105 logZ.set_s( log(Z) ) ; 106 107 // We have to compute the interaction matrix corresponding to the feature. 108 vpMatrix LlogZ(1,6) ; 109 LlogZ[0][0] = LlogZ[0][1] = LlogZ[0][5] = 0 ; 110 LlogZ[0][2] = -1/Z; 111 LlogZ[0][3] = -y; 112 LlogZ[0][4] = x; 113 logZ.setInteractionMatrix(LlogZ) ; 114 115 116 // compute the control law 117 vpColVector v = task.computeControlLaw(); // camera velocity 118 } 119 return 0; 120 } 121 \endcode 122 123 The second example shows how to create and use a feature whose specificity is 124 to have a desired feature fixed to zero. It is the case for the feature \f$ 125 log( \frac{Z}{Z^*}) \f$. 126 127 \code 128 #include <visp3/core/vpGenericFeature.h> 129 #include <visp3/vs/vpServo.h> 130 131 int main() 132 { 133 vpServo task; // Visual servoing task 134 135 //First we have to define the desired feature log(Z*) corresponding to the desired point. 136 double xd = 0; //The x coordinate of the desired point. 137 double yd = 0; //The y coordinate of the desired point. 138 double Zd = 1; //The depth of the desired point. 139 140 //Then we have to define the current feature log(Z) corresponding to the current point. 141 double x = 1; //The x coordinate of the current point. 142 double y = 1; //The y coordinate of the current point. 143 double Z = 2; //The depth of the current point. 144 vpGenericFeature logZ(1); //The dimension of the feature is 1. 145 logZ.set_s( log(Z/Zd) ); 146 147 // Set eye-in-hand control law. 148 // The computed velocities will be expressed in the camera frame 149 task.setServo(vpServo::EYEINHAND_CAMERA); 150 // Interaction matrix is computed with the current visual features sd 151 task.setInteractionMatrixType(vpServo::CURRENT); 152 153 // Add the point feature to the task 154 task.addFeature(logZ); 155 156 // Control loop 157 for ( ; ; ) { 158 // The new parameters x, y and Z must be computed here. 159 160 // Update the current point visual feature 161 logZ.set_s( log(Z/Zd) ) ; 162 163 // We have to compute the interaction matrix corresponding to the feature. 164 vpMatrix LlogZ(1,6) ; 165 LlogZ[0][0] = LlogZ[0][1] = LlogZ[0][5] = 0 ; 166 LlogZ[0][2] = -1/Z; 167 LlogZ[0][3] = -y; 168 LlogZ[0][4] = x; 169 logZ.setInteractionMatrix(LlogZ) ; 170 171 172 // compute the control law 173 vpColVector v = task.computeControlLaw(); // camera velocity 174 } 175 return 0; 176 } 177 \endcode 178 179 If the feature needs to be use with other features, the example 180 servoSimuPoint2DhalfCamVelocity2.cpp shows how to do it. 181 */ 182 class VISP_EXPORT vpGenericFeature : public vpBasicFeature 183 { 184 private: 185 vpGenericFeature(); 186 187 public: 188 explicit vpGenericFeature(unsigned int dim); 189 virtual ~vpGenericFeature(); 190 191 void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green, 192 unsigned int thickness = 1) const; 193 void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green, 194 unsigned int thickness = 1) const; 195 196 vpGenericFeature *duplicate() const; 197 198 vpColVector error(const vpBasicFeature &s_star, unsigned int select = FEATURE_ALL); 199 200 vpColVector error(unsigned int select = FEATURE_ALL); 201 getInteractionMatrix()202 vpMatrix getInteractionMatrix() const { return L; } 203 void get_s(vpColVector &s) const; 204 void get_s(double &s0) const; 205 void get_s(double &s0, double &s1) const; 206 void get_s(double &s0, double &s1, double &s2) const; 207 208 void init(); 209 210 vpMatrix interaction(unsigned int select = FEATURE_ALL); 211 212 void print(unsigned int select = FEATURE_ALL) const; 213 void setInteractionMatrix(const vpMatrix &L); 214 void setError(const vpColVector &error_vector); 215 void set_s(const vpColVector &s); 216 void set_s(const double s0); 217 void set_s(const double s0, const double s1); 218 void set_s(const double s0, const double s1, const double s2); 219 220 private: 221 typedef enum { errorNotInitalized, errorInitialized, errorHasToBeUpdated } vpGenericFeatureErrorType; 222 223 vpMatrix L; 224 vpColVector err; 225 vpGenericFeatureErrorType errorStatus; 226 }; 227 228 #endif 229