Visual Servoing Platform version 3.5.0
vpFeatureMomentGravityCenter.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 for all supported moment features.
33 *
34 * Authors:
35 * Filip Novotny
36 *
37 *****************************************************************************/
38
39#include <visp3/core/vpConfig.h>
40
41#ifdef VISP_MOMENTS_COMBINE_MATRICES
42
43#include <limits>
44#include <vector>
45
46#include <visp3/core/vpMomentObject.h>
47#include <visp3/visual_features/vpFeatureMomentBasic.h>
48#include <visp3/visual_features/vpFeatureMomentDatabase.h>
49#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
50
60{
61 bool found_featuremoment_basic;
62
63 vpFeatureMomentBasic &featureMomentBasic = (static_cast<vpFeatureMomentBasic &>(
64 featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic)));
65 const vpMomentObject &momentObject = moment->getObject();
66
67 if (!found_featuremoment_basic)
68 throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found");
69
70 interaction_matrices[0].resize(1, 6);
71 interaction_matrices[1].resize(1, 6);
72
74 featureMomentBasic.interaction(1, 0) / momentObject.get(0, 0) -
75 momentObject.get(1, 0) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
77 featureMomentBasic.interaction(0, 1) / momentObject.get(0, 0) -
78 momentObject.get(0, 1) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
79}
80
81#else
82
83#include <limits>
84#include <vector>
85
86#include <visp3/core/vpMomentCentered.h>
87#include <visp3/core/vpMomentDatabase.h>
88#include <visp3/core/vpMomentGravityCenter.h>
89#include <visp3/core/vpMomentObject.h>
90#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
91
102{
103 bool found_moment_centered;
104 bool found_moment_gravity;
105
106 const vpMomentCentered &momentCentered =
107 (static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered)));
108 const vpMomentGravityCenter &momentGravity =
109 static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
110
111 const vpMomentObject &momentObject = moment->getObject();
112
113 if (!found_moment_centered)
114 throw vpException(vpException::notInitialized, "vpMomentCentered not found");
115 if (!found_moment_gravity)
116 throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
117
118 interaction_matrices[0].resize(1, 6);
119 interaction_matrices[1].resize(1, 6);
120 int epsilon;
121 if (momentObject.getType() == vpMomentObject::DISCRETE) {
122 epsilon = 1;
123 } else {
124 epsilon = 4;
125 }
126 double n11 = momentCentered.get(1, 1) / momentObject.get(0, 0);
127 double n20 = momentCentered.get(2, 0) / momentObject.get(0, 0);
128 double n02 = momentCentered.get(0, 2) / momentObject.get(0, 0);
129 double Xg = momentGravity.getXg();
130 double Yg = momentGravity.getYg();
131 double Zg_inv = A * Xg + B * Yg + C;
132 double Xgvz = Xg * Zg_inv + A * epsilon * n20 + B * epsilon * n11;
133 double Ygvz = Yg * Zg_inv + A * epsilon * n11 + B * epsilon * n02;
134 double Xgwx = Xg * Yg + epsilon * n11;
135 double Ygwy = -Xgwx;
136 double Xgwy = -(1 + Xg * Xg + epsilon * n20);
137 double Ygwx = 1 + Yg * Yg + epsilon * n02;
138
139 int VX = 0;
140 int VY = 1;
141 int VZ = 2;
142 int WX = 3;
143 int WY = 4;
144 int WZ = 5;
145
146 interaction_matrices[0][0][VX] = -Zg_inv;
147 interaction_matrices[0][0][VY] = 0;
148 interaction_matrices[0][0][VZ] = Xgvz;
149
150 interaction_matrices[0][0][WX] = Xgwx;
151 interaction_matrices[0][0][WY] = Xgwy;
152 interaction_matrices[0][0][WZ] = Yg;
153
154 interaction_matrices[1][0][VX] = 0;
155 interaction_matrices[1][0][VY] = -Zg_inv;
156 interaction_matrices[1][0][VZ] = Ygvz;
157
158 interaction_matrices[1][0][WX] = Ygwx;
159 interaction_matrices[1][0][WY] = Ygwy;
160 interaction_matrices[1][0][WZ] = -Xg;
161}
162
163#endif
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 basic moment feature. Computes the interaction matrix associated with v...
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
This class defines the double-indexed centered moment descriptor .
double get(unsigned int i, unsigned int j) const
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