Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
vpFeatureMomentArea.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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> // numeric_limits
40 #include <vector>
41 #include <visp3/core/vpConfig.h>
42 
43 #include <visp3/core/vpMomentArea.h>
44 #include <visp3/core/vpMomentGravityCenter.h>
45 #include <visp3/core/vpMomentObject.h>
46 #include <visp3/visual_features/vpFeatureMomentArea.h>
47 #include <visp3/visual_features/vpFeatureMomentCentered.h>
48 #include <visp3/visual_features/vpFeatureMomentDatabase.h>
49 
50 BEGIN_VISP_NAMESPACE
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 = static_cast<const vpFeatureMomentCentered &>(
73  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  }
78  else {
79  // Get Xg and Yg
80  bool found_xgyg;
81  const vpMomentGravityCenter &momentGravity =
82  static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_xgyg));
83  if (!found_xgyg)
84  throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
85 
86  bool found_m00;
87  const vpMomentArea &areamoment = static_cast<const vpMomentArea &>(moments.get("vpMomentArea", found_m00));
88  if (!found_m00)
89  throw vpException(vpException::notInitialized, "vpMomentArea not found");
90 
91  double Xg = momentGravity.getXg();
92  double Yg = momentGravity.getYg();
93 
94  double a = areamoment.get()[0]; // Area scalar
95 
96  assert(std::fabs(a - mobj.get(0, 0)) < a * std::numeric_limits<double>::epsilon());
97 
98  interaction_matrices[0][0][0] = -a * A;
99  interaction_matrices[0][0][1] = -a * B;
100  interaction_matrices[0][0][2] = (3 * a) * (A * Xg + B * Yg) + (2 * C * a);
101  interaction_matrices[0][0][3] = 3 * a * Yg;
102  interaction_matrices[0][0][4] = -3 * a * Xg;
103  interaction_matrices[0][0][5] = 0.;
104  }
105 }
106 END_VISP_NAMESPACE
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:74
void compute_interaction() VP_OVERRIDE
Functionality computation for centered moment feature. Computes the interaction matrix associated wit...
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
vpFeatureMoment & get(const std::string &feature_name, bool &found)
std::vector< vpMatrix > interaction_matrices
vpFeatureMomentDatabase * featureMomentsDataBase
vpMomentDatabase & moments
const vpMoment * moment
Class handling the surface moment.
Definition: vpMomentArea.h:59
const vpMoment & get(const std::string &moment_name, bool &found) const
Class describing 2D gravity center moment.
Class for generic objects.
const std::vector< double > & get() const
vpObjectType getType() const
const vpMomentObject & getObject() const
Definition: vpMoment.h:145
const std::vector< double > & get() const
Definition: vpMoment.h:150