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 * Pre-filled pseudo-database used to handle dependencies between common 33 *moment features. 34 * 35 * Authors: 36 * Filip Novotny 37 * 38 *****************************************************************************/ 39 40 /*! 41 \file vpFeatureMomentCommon.h 42 \brief Pre-filled pseudo-database used to handle dependencies between common 43 moment features. 44 */ 45 46 #ifndef _vpFeatureMomentCommon_h_ 47 #define _vpFeatureMomentCommon_h_ 48 #include <visp3/visual_features/vpFeatureMomentAlpha.h> 49 #include <visp3/visual_features/vpFeatureMomentArea.h> 50 #include <visp3/visual_features/vpFeatureMomentAreaNormalized.h> 51 #include <visp3/visual_features/vpFeatureMomentBasic.h> 52 #include <visp3/visual_features/vpFeatureMomentCInvariant.h> 53 #include <visp3/visual_features/vpFeatureMomentCentered.h> 54 #include <visp3/visual_features/vpFeatureMomentCommon.h> 55 #include <visp3/visual_features/vpFeatureMomentDatabase.h> 56 #include <visp3/visual_features/vpFeatureMomentGravityCenter.h> 57 #include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h> 58 59 class vpMomentDatabase; 60 class vpServo; 61 /*! 62 \class vpFeatureMomentCommon 63 64 \ingroup group_visual_features 65 66 \brief This class allows to access common vpFeatureMoments in a pre-filled 67 database. 68 69 It is a vpMomentDatabase filled with the following moments: 70 - vpFeatureMomentGravityCenter 71 - vpFeatureMomentGravityCenterNormalized 72 - vpFeatureMomentAreaNormalized 73 - vpFeatureMomentCInvariant 74 - vpFeatureMomentAlpha 75 - vpFeatureMomentCentered 76 - vpFeatureMomentBasic 77 78 79 There is no need to do the linkTo operations manually nor is it necessary to 80 care about the order of feature computation. 81 82 This class has an vpMomentCommon::updateAll method capable of updating the 83 plane parameters AND computing interaction matrices inside the features. 84 85 The moment features computed by this class are classical moments 86 features used in moment-based visual servoing. For more 87 information see \cite Tahri05z. 88 89 To initialize this feature set, the user needs to supply a vpMomentDatabase 90 containing at least the contents of vpMomentCommon. 91 92 The features can be retrieved like from a normal vpFeatureMomentDatabase. 93 However, some shortcuts to retrieve the features are provided. 94 95 \attention Make sure your object is at least of order 6 when using this 96 pre-filled database. 97 98 The following code demonstrates the construction of a 6x6 interaction matrix 99 as described in [1]. 100 \code 101 #include <visp3/core/vpMomentObject.h> 102 #include <visp3/core/vpMomentCommon.h> 103 #include <visp3/core/vpPoint.h> 104 #include <visp3/visual_features/vpFeatureMoment.h> 105 #include <visp3/visual_features/vpFeatureMomentCommon.h> 106 #include <visp3/vs/vpServo.h> 107 #include <iostream> 108 #include <vector> 109 110 int main() 111 { 112 // Define source polygon 113 vpPoint p; 114 std::vector<vpPoint> vec_p; // vector that contains the vertices of the contour polygon 115 116 p.set_x(-0.2); p.set_y(0.1); // coordinates in meters in the image plane (vertex 1) 117 vec_p.push_back(p); 118 p.set_x(+0.3); p.set_y(0.1); // coordinates in meters in the image plane (vertex 2) 119 vec_p.push_back(p); 120 p.set_x(+0.2); p.set_y(-0.1); // coordinates in meters in the image plane (vertex 3) 121 vec_p.push_back(p); 122 p.set_x(-0.2); p.set_y(-0.15); // coordinates in meters in the image plane (vertex 4) 123 vec_p.push_back(p); 124 p.set_x(-0.2); p.set_y(0.1); // close the contour (vertex 5 = vertex 1) 125 vec_p.push_back(p); 126 127 128 vpMomentObject src(6); // Create a source moment object with 6 as maximum order 129 src.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon 130 src.fromVector(vec_p); // Init the dense object with the source polygon 131 vec_p.clear(); 132 133 //Define destination polygon. This is the source polygon translated 134 //of 0.1 on x-axis 135 p.set_x(-0.1); p.set_y(0.1); // coordinates in meters in the image plane (vertex 1) 136 vec_p.push_back(p); 137 p.set_x(+0.4); p.set_y(0.1); // coordinates in meters in the image plane (vertex 2) 138 vec_p.push_back(p); 139 p.set_x(+0.3); p.set_y(-0.1); // coordinates in meters in the image plane (vertex 3) 140 vec_p.push_back(p); 141 p.set_x(-0.1); p.set_y(-0.15); // coordinates in meters in the image plane (vertex 4) 142 vec_p.push_back(p); 143 p.set_x(-0.1); p.set_y(0.1); // close the contour (vertex 5 = vertex 1) 144 vec_p.push_back(p); 145 146 vpMomentObject dst(6); // Create a destination moment object with 6 as maximum order 147 dst.setType(vpMomentObject::DENSE_POLYGON); // The object is defined by a countour polygon 148 dst.fromVector(vec_p); // Init the dense object with the destination 149 // polygon 150 151 152 //init classic moment primitives (for source) 153 vpMomentCommon mdb_src(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),1.); 154 //Init classic features 155 vpFeatureMomentCommon fmdb_src(mdb_src); 156 157 ////init classic moment primitives (for destination) 158 vpMomentCommon mdb_dst(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),1.); 159 //Init classic features 160 vpFeatureMomentCommon fmdb_dst(mdb_dst); 161 162 //update+compute moment primitives from object (for source) 163 mdb_src.updateAll(src); 164 //update+compute features (+interaction matrices) from plane 165 fmdb_src.updateAll(0.,0.,1.); 166 167 //update+compute moment primitives from object (for destination) 168 mdb_dst.updateAll(dst); 169 //update+compute features (+interaction matrices) from plane 170 fmdb_dst.updateAll(0.,0.,1.); 171 172 //define visual servoing task 173 vpServo task; 174 task.setServo(vpServo::EYEINHAND_CAMERA); 175 task.setInteractionMatrixType(vpServo::CURRENT); 176 177 //Add all classic features to the task 178 //In this example, source and destination features are translated by 0.1 179 //will produce a movement of 0.1 on x-axis. 180 task.addFeature(fmdb_src.getFeatureGravityNormalized(),fmdb_dst.getFeatureGravityNormalized()); 181 task.addFeature(fmdb_src.getFeatureAn(),fmdb_dst.getFeatureAn()); 182 //the object is NOT symmetric 183 //select C4 and C6 184 task.addFeature(fmdb_src.getFeatureCInvariant(),fmdb_dst.getFeatureCInvariant(),(1 << 3) | (1 << 5)); 185 task.addFeature(fmdb_src.getFeatureAlpha(),fmdb_dst.getFeatureAlpha()); 186 187 task.setLambda(1) ; 188 vpColVector v = task.computeControlLaw() ; 189 190 task.print(); 191 192 return 0; 193 } 194 \endcode 195 This code produces the following output: 196 \code 197 Visual servoing task: 198 Type of control law 199 Eye-in-hand configuration 200 Control in the camera frame 201 List of visual features : s 202 0.0166667,-0.00833333, 203 1, 204 -0.312148,0.0249916, 205 -1.43449, 206 List of desired visual features : s* 207 0.116667,-0.00833333, 208 1, 209 -0.312148,0.0249916, 210 -1.43449, 211 Interaction Matrix Ls 212 -1 0 -6.938893904e-18 0.007291666667 -1.06875 -0.008333333333 213 0 -1 3.469446952e-18 1.0171875 -0.007291666667 -0.01666666667 214 0 0 -1 0.0125 0.025 0 215 0 0 -4.585529113e-15 -0.2983860943 0.5832596643 -4.376751552e-16 216 0 0 -3.58244462e-15 0.08633028234 -0.2484618767 3.63421192e-16 217 4.353086256e-17 -1.339411156e-16 -0 -0.03019436997 -0.0168230563 -1 218 Error vector (s-s*) 219 -0.1 0 0 1.831867991e-15 -1.072059108e-15 0 220 Gain : Zero= 1 Inf= 1 Deriv= 0 221 222 \endcode 223 */ 224 class VISP_EXPORT vpFeatureMomentCommon : public vpFeatureMomentDatabase 225 { 226 private: 227 vpFeatureMomentGravityCenter featureGravity; 228 vpFeatureMomentGravityCenterNormalized featureGravityNormalized; 229 vpFeatureMomentAreaNormalized featureAn; 230 vpFeatureMomentCInvariant featureCInvariant; 231 vpFeatureMomentAlpha featureAlpha; 232 vpFeatureMomentCentered featureCentered; 233 vpFeatureMomentBasic featureMomentBasic; 234 vpFeatureMomentArea feature_moment_area; 235 236 public: 237 vpFeatureMomentCommon(vpMomentDatabase &moments, double A = 0.0, double B = 0.0, double C = 1.0); 238 void updateAll(double A, double B, double C); 239 /*! 240 Returns alpha. 241 */ getFeatureAlpha()242 vpFeatureMomentAlpha &getFeatureAlpha() { return featureAlpha; } 243 244 /*! 245 Returns normalized surface. 246 */ getFeatureAn()247 vpFeatureMomentAreaNormalized &getFeatureAn() { return featureAn; } 248 /*! 249 Returns basic moment. 250 */ getFeatureMomentBasic()251 vpFeatureMomentBasic &getFeatureMomentBasic() { return featureMomentBasic; } 252 /*! 253 Returns centered moments. 254 */ getFeatureCentered()255 vpFeatureMomentCentered &getFeatureCentered() { return featureCentered; } 256 257 /*! 258 Returns non-symmetric invariants. 259 */ getFeatureCInvariant()260 vpFeatureMomentCInvariant &getFeatureCInvariant() { return featureCInvariant; } 261 /*! 262 Returns normalized gravity center. 263 */ getFeatureGravityNormalized()264 vpFeatureMomentGravityCenterNormalized &getFeatureGravityNormalized() { return featureGravityNormalized; } 265 /*! 266 Returns the area 267 */ getFeatureArea()268 vpFeatureMomentArea &getFeatureArea() { return feature_moment_area; } 269 /*! 270 Returns gravity center 271 */ getFeatureGravityCenter()272 vpFeatureMomentGravityCenter &getFeatureGravityCenter() { return featureGravity; } 273 }; 274 275 #endif 276