Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeatureMomentBasic.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 for all supported moment features.
33 *
34 * Authors:
35 * Filip Novotny
36 *
37*****************************************************************************/
38
39#include <limits>
40#include <vector>
41#include <visp3/core/vpMomentObject.h>
42#include <visp3/visual_features/vpFeatureMomentBasic.h>
43#include <visp3/visual_features/vpFeatureMomentDatabase.h>
53vpFeatureMomentBasic::vpFeatureMomentBasic(vpMomentDatabase &data_base, double A_, double B_, double C_,
54 vpFeatureMomentDatabase *featureMoments)
55 : vpFeatureMoment(data_base, A_, B_, C_, featureMoments), order(0)
56{
57}
58
64{
65 int delta;
66 const vpMomentObject &m = moment->getObject();
67 order = m.getOrder() + 1;
69 for (std::vector<vpMatrix>::iterator i = interaction_matrices.begin(); i != interaction_matrices.end(); ++i)
70 i->resize(1, 6);
72 delta = 0;
73 } else {
74 delta = 1;
75 }
76
77 int VX = 0;
78 int VY = 1;
79 int VZ = 2;
80 int WX = 3;
81 int WY = 4;
82 int WZ = 5;
83
84 // i=0;j=0
85 interaction_matrices[0][0][VX] = -delta * A * m.get(0, 0);
86 interaction_matrices[0][0][VY] = -delta * B * m.get(0, 0);
87 interaction_matrices[0][0][VZ] =
88 3 * delta * (A * m.get(1, 0) + B * m.get(0, 1) + C * m.get(0, 0)) - delta * C * m.get(0, 0);
89
90 interaction_matrices[0][0][WX] = 3 * delta * m.get(0, 1);
91 interaction_matrices[0][0][WY] = -3 * delta * m.get(1, 0);
92 interaction_matrices[0][0][WZ] = 0;
93
94 // int i=0;
95 for (int j = 1; j < (int)order - 1; j++) {
96 unsigned int j_ = (unsigned int)j;
97 unsigned int jm1_ = j_ - 1;
98 unsigned int jp1_ = j_ + 1;
99
100 interaction_matrices[j_ * order][0][VX] = -delta * A * m.get(0, j_);
101 interaction_matrices[j_ * order][0][VY] =
102 -j * (A * m.get(1, jm1_) + B * m.get(0, j_) + C * m.get(0, jm1_)) - delta * B * m.get(0, j_);
103 interaction_matrices[j_ * order][0][VZ] =
104 (j + 3 * delta) * (A * m.get(1, j_) + B * m.get(0, jp1_) + C * m.get(0, j_)) - delta * C * m.get(0, j_);
105
106 interaction_matrices[j_ * order][0][WX] = (j + 3 * delta) * m.get(0, jp1_) + j * m.get(0, jm1_);
107 interaction_matrices[j_ * order][0][WY] = -(j + 3 * delta) * m.get(1, j_);
108 interaction_matrices[j_ * order][0][WZ] = -j * m.get(1, jm1_);
109 }
110
111 // int j=0;
112 for (int i = 1; i < (int)order - 1; i++) {
113 unsigned int i_ = (unsigned int)i;
114 unsigned int im1_ = i_ - 1;
115 unsigned int ip1_ = i_ + 1;
116
117 interaction_matrices[i_][0][VX] =
118 -i * (A * m.get(i_, 0) + B * m.get(im1_, 1) + C * m.get(im1_, 0)) - delta * A * m.get(i_, 0);
119 interaction_matrices[i_][0][VY] = -delta * B * m.get(i_, 0);
120 interaction_matrices[i_][0][VZ] =
121 (i + 3 * delta) * (A * m.get(ip1_, 0) + B * m.get(i_, 1) + C * m.get(i_, 0)) - delta * C * m.get(i_, 0);
122
123 interaction_matrices[i_][0][WX] = (i + 3 * delta) * m.get(i_, 1);
124 interaction_matrices[i_][0][WY] = -(i + 3 * delta) * m.get(ip1_, 0) - i * m.get(im1_, 0);
125 interaction_matrices[i_][0][WZ] = i * m.get(im1_, 1);
126 }
127
128 for (int j = 1; j < (int)order - 1; j++) {
129 unsigned int j_ = (unsigned int)j;
130 unsigned int jm1_ = j_ - 1;
131 unsigned int jp1_ = j_ + 1;
132
133 for (int i = 1; i < (int)order - j - 1; i++) {
134 unsigned int i_ = (unsigned int)i;
135 unsigned int im1_ = i_ - 1;
136 unsigned int ip1_ = i_ + 1;
137
138 interaction_matrices[j_ * order + i_][0][VX] =
139 -i * (A * m.get(i_, j_) + B * m.get(im1_, jp1_) + C * m.get(im1_, j_)) - delta * A * m.get(i_, j_);
140 interaction_matrices[j_ * order + i_][0][VY] =
141 -j * (A * m.get(ip1_, jm1_) + B * m.get(i_, j_) + C * m.get(i_, jm1_)) - delta * B * m.get(i_, j_);
142 interaction_matrices[j_ * order + i_][0][VZ] =
143 (i + j + 3 * delta) * (A * m.get(ip1_, j_) + B * m.get(i_, jp1_) + C * m.get(i_, j_)) -
144 delta * C * m.get(i_, j_);
145
146 interaction_matrices[j_ * order + i_][0][WX] = (i + j + 3 * delta) * m.get(i_, jp1_) + j * m.get(i_, jm1_);
147 interaction_matrices[j_ * order + i_][0][WY] = -(i + j + 3 * delta) * m.get(ip1_, j_) - i * m.get(im1_, j_);
148 interaction_matrices[j_ * order + i_][0][WZ] = i * m.get(im1_, jp1_) - j * m.get(ip1_, jm1_);
149 }
150 }
151}
152
159vpMatrix vpFeatureMomentBasic::interaction(unsigned int select_one, unsigned int select_two) const
160{
161 if (select_one + select_two > moment->getObject().getOrder())
162 throw vpException(vpException::badValue, "The requested value has not "
163 "been computed, you should "
164 "specify a higher order.");
165 return interaction_matrices[select_two * order + select_one];
166}
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
vpFeatureMomentBasic(vpMomentDatabase &moments, double A, double B, double C, vpFeatureMomentDatabase *featureMoments=NULL)
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
This class allows to register all feature moments (implemented in vpFeatureMoment....
This class defines shared system methods/attributes for 2D moment features but no functional code....
std::vector< vpMatrix > interaction_matrices
const vpMoment * moment
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
This class allows to register all vpMoments so they can access each other according to their dependen...
Class for generic objects.
unsigned int getOrder() const
const std::vector< double > & get() const
vpObjectType getType() const
const vpMomentObject & getObject() const
Definition vpMoment.h:145