Visual Servoing Platform  version 3.2.1 under development (2019-10-21) under development (2019-10-21)
vpFeatureMomentArea.cpp
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  * Implementation of vpFeatureMomentArea associated to vpMomentArea
33  *
34  * Authors:
35  * Manikandan Bakthavatchalam
36  *
37  *****************************************************************************/
38 #include <cassert>
39 #include <limits>
40 #include <limits> // numeric_limits
41 #include <vector>
42 #include <visp3/core/vpConfig.h>
43 
44 #include <visp3/core/vpMomentArea.h>
45 #include <visp3/core/vpMomentGravityCenter.h>
46 #include <visp3/core/vpMomentObject.h>
47 #include <visp3/visual_features/vpFeatureMomentArea.h>
48 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
49 
58 {
59 
60  interaction_matrices.resize(1);
61  interaction_matrices[0].resize(1, 6);
62 
63  // Retreive the moment object associated with this feature
65  if (mobj.getType() == vpMomentObject::DISCRETE) {
66  /*
67  * The interaction matrix for the discrete case is zero
68  * since the feature m00 is constant.
69  * Refer thesis of Omar Tahri 2005 [Section 3.4.22]
70  */
71  interaction_matrices[0][0][0] = 0.;
72  interaction_matrices[0][0][1] = 0.;
73  interaction_matrices[0][0][2] = 0.;
74  interaction_matrices[0][0][3] = 0.;
75  interaction_matrices[0][0][4] = 0.;
76  interaction_matrices[0][0][5] = 0.;
77  } else {
78  // Get Xg and Yg
79  bool found_xgyg;
80  const vpMomentGravityCenter &momentGravity =
81  static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_xgyg));
82  if (!found_xgyg)
83  throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
84 
85  bool found_m00;
86  const vpMomentArea &areamoment = static_cast<const vpMomentArea &>(moments.get("vpMomentArea", found_m00));
87  if (!found_m00)
88  throw vpException(vpException::notInitialized, "vpMomentArea not found");
89 
90  double Xg = momentGravity.getXg();
91  double Yg = momentGravity.getYg();
92 
93  double a = areamoment.get()[0]; // Area scalar
94 
95  assert(std::fabs(a - mobj.get(0, 0)) < a * std::numeric_limits<double>::epsilon());
96 
97  interaction_matrices[0][0][0] = -a * A;
98  interaction_matrices[0][0][1] = -a * B;
99  interaction_matrices[0][0][2] = (3 * a) * (A * Xg + B * Yg) + (2 * C * a);
100  interaction_matrices[0][0][3] = 3 * a * Yg;
101  interaction_matrices[0][0][4] = -3 * a * Xg;
102  interaction_matrices[0][0][5] = 0.;
103  }
104 }
const vpMoment * moment
const vpMoment & get(const char *type, bool &found) const
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class for generic objects.
std::vector< vpMatrix > interaction_matrices
const std::vector< double > & get() const
Definition: vpMoment.h:155
Class handling the surface moment.
Definition: vpMomentArea.h:54
const vpMomentObject & getObject() const
Definition: vpMoment.h:150
vpMomentDatabase & moments
const std::vector< double > & get() const
vpObjectType getType() const
Class describing 2D gravity center moment.
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98