Visual Servoing Platform version 3.5.0
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}
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
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 char *type, bool &found)
std::vector< vpMatrix > interaction_matrices
vpFeatureMomentDatabase * featureMomentsDataBase
vpMomentDatabase & moments
const vpMoment * moment
Class handling the surface moment.
Definition: vpMomentArea.h:63
const vpMoment & get(const char *type, 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:150
const std::vector< double > & get() const
Definition: vpMoment.h:155