Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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/vpFeatureMomentCentered.h>
49 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
50 
62 {
63 
64  interaction_matrices.resize(1);
65  interaction_matrices[0].resize(1, 6);
66 
67  // Retreive the moment object associated with this feature
69  if (mobj.getType() == vpMomentObject::DISCRETE) {
70  // Get centered moments
71  bool found_centered;
72  const vpFeatureMomentCentered &momentCentered =
73  static_cast<const vpFeatureMomentCentered &>(featureMomentsDataBase->get("vpFeatureMomentCentered", found_centered));
74  if (!found_centered)
75  throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
76  interaction_matrices[0] = momentCentered.interaction(2,0) + momentCentered.interaction(0,2);
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
Functionality computation for centered moment feature. Computes the interaction matrix associated wit...
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:62
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.
vpFeatureMomentDatabase * featureMomentsDataBase
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
vpFeatureMoment & get(const char *type, bool &found)