Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeatureMomentCInvariant.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#include <visp3/core/vpMomentObject.h>
39#ifdef VISP_MOMENTS_COMBINE_MATRICES
40#include <visp3/core/vpMomentCInvariant.h>
41#include <visp3/core/vpMomentCentered.h>
42#include <visp3/visual_features/vpFeatureMomentBasic.h>
43#include <visp3/visual_features/vpFeatureMomentCInvariant.h>
44#include <visp3/visual_features/vpFeatureMomentCentered.h>
45
46#include <visp3/visual_features/vpFeatureMomentDatabase.h>
47
48#include <limits>
49#include <vector>
50
61{
62 std::vector<vpMatrix> LI(16);
63 bool found_moment_centered;
64 bool found_moment_cinvariant;
65 bool found_FeatureMoment_centered;
66 bool found_featuremoment_basic;
67
68 const vpMomentObject &momentObject = moment->getObject();
69 const vpMomentCentered &momentCentered =
70 (static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered)));
71 const vpMomentCInvariant &momentCInvariant =
72 (static_cast<const vpMomentCInvariant &>(moments.get("vpMomentCInvariant", found_moment_cinvariant)));
73 vpFeatureMomentCentered &featureMomentCentered = (static_cast<vpFeatureMomentCentered &>(
74 featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered)));
75
76 vpFeatureMomentBasic &featureMomentBasic = (static_cast<vpFeatureMomentBasic &>(
77 featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic)));
78
79 if (!found_featuremoment_basic)
80 throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found");
81
82 if (!found_moment_centered)
83 throw vpException(vpException::notInitialized, "vpMomentCentered not found");
84 if (!found_moment_cinvariant)
85 throw vpException(vpException::notInitialized, "vpMomentCInvariant not found");
86 if (!found_FeatureMoment_centered)
87 throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
88
89 vpMatrix zeros(1, 6);
90 for (int i = 0; i < 6; i++)
91 zeros[0][i] = 0;
92
93 LI[1] = -featureMomentCentered.interaction(2, 0) * momentCentered.get(0, 2) -
94 momentCentered.get(2, 0) * featureMomentCentered.interaction(0, 2) +
95 2 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
96
97 LI[2] = 2 * (momentCentered.get(2, 0) - momentCentered.get(0, 2)) *
98 (featureMomentCentered.interaction(2, 0) - featureMomentCentered.interaction(0, 2)) +
99 8 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
100
101 LI[3] = 2 * (momentCentered.get(3, 0) - 3 * momentCentered.get(1, 2)) *
102 (featureMomentCentered.interaction(3, 0) - 3 * featureMomentCentered.interaction(1, 2)) +
103 2 * (3 * momentCentered.get(2, 1) - momentCentered.get(0, 3)) *
104 (3 * featureMomentCentered.interaction(2, 1) - featureMomentCentered.interaction(0, 3));
105
106 LI[4] = 2 * (momentCentered.get(3, 0) + momentCentered.get(1, 2)) *
107 (featureMomentCentered.interaction(3, 0) + featureMomentCentered.interaction(1, 2)) +
108 2 * (momentCentered.get(2, 1) + momentCentered.get(0, 3)) *
109 (featureMomentCentered.interaction(2, 1) + featureMomentCentered.interaction(0, 3));
110
111 LI[5] = -2 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(3, 0) +
112 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(2, 1) *
113 featureMomentCentered.interaction(3, 0) -
114 4 * pow(momentCentered.get(1, 2), 3) * featureMomentCentered.interaction(3, 0) +
115 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(3, 0) *
116 featureMomentCentered.interaction(2, 1) -
117 12 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(2, 1) +
118 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(2, 1) +
119 6 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * momentCentered.get(3, 0) *
120 featureMomentCentered.interaction(1, 2) -
121 12 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) +
122 6 * momentCentered.get(1, 2) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(1, 2) -
123 2 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(0, 3) +
124 6 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * momentCentered.get(3, 0) *
125 featureMomentCentered.interaction(0, 3) -
126 4 * pow(momentCentered.get(2, 1), 3) * featureMomentCentered.interaction(0, 3);
127
128 LI[6] = 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(3, 0) +
129 4 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(3, 0) -
130 6 * momentCentered.get(1, 2) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(3, 0) -
131 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(2, 1) *
132 featureMomentCentered.interaction(3, 0) +
133 2 * pow(momentCentered.get(1, 2), 3) * featureMomentCentered.interaction(3, 0) -
134 12 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * momentCentered.get(3, 0) *
135 featureMomentCentered.interaction(2, 1) -
136 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(3, 0) *
137 featureMomentCentered.interaction(2, 1) +
138 12 * pow(momentCentered.get(2, 1), 3) * featureMomentCentered.interaction(2, 1) +
139 6 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(2, 1) +
140 6 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(2, 1) -
141 6 * momentCentered.get(0, 3) * pow(momentCentered.get(1, 2), 2) * featureMomentCentered.interaction(2, 1) +
142 6 * momentCentered.get(1, 2) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(1, 2) -
143 6 * pow(momentCentered.get(2, 1), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) -
144 6 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * momentCentered.get(3, 0) *
145 featureMomentCentered.interaction(1, 2) +
146 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) -
147 12 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(2, 1) *
148 featureMomentCentered.interaction(1, 2) +
149 12 * pow(momentCentered.get(1, 2), 3) * featureMomentCentered.interaction(1, 2) +
150 4 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(0, 3) -
151 6 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * momentCentered.get(3, 0) *
152 featureMomentCentered.interaction(0, 3) +
153 2 * pow(momentCentered.get(2, 1), 3) * featureMomentCentered.interaction(0, 3) +
154 6 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(0, 3) -
155 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(0, 3);
156
157 LI[7] = -3 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(3, 0) +
158 6 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * momentCentered.get(3, 0) *
159 featureMomentCentered.interaction(3, 0) -
160 2 * pow(momentCentered.get(2, 1), 3) * featureMomentCentered.interaction(3, 0) -
161 3 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(3, 0) +
162 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(3, 0) +
163 3 * momentCentered.get(0, 3) * pow(momentCentered.get(1, 2), 2) * featureMomentCentered.interaction(3, 0) +
164 pow(momentCentered.get(0, 3), 3) * featureMomentCentered.interaction(3, 0) +
165 3 * momentCentered.get(1, 2) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(2, 1) -
166 6 * pow(momentCentered.get(2, 1), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) -
167 6 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * momentCentered.get(3, 0) *
168 featureMomentCentered.interaction(2, 1) +
169 6 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) -
170 9 * momentCentered.get(1, 2) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(2, 1) -
171 12 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(2, 1) *
172 featureMomentCentered.interaction(2, 1) +
173 3 * pow(momentCentered.get(1, 2), 3) * featureMomentCentered.interaction(2, 1) -
174 3 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(1, 2) * featureMomentCentered.interaction(2, 1) +
175 3 * momentCentered.get(2, 1) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(1, 2) +
176 12 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * momentCentered.get(3, 0) *
177 featureMomentCentered.interaction(1, 2) +
178 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(3, 0) *
179 featureMomentCentered.interaction(1, 2) -
180 3 * pow(momentCentered.get(2, 1), 3) * featureMomentCentered.interaction(1, 2) -
181 6 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(1, 2) +
182 9 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(1, 2) -
183 3 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(1, 2) +
184 6 * momentCentered.get(0, 3) * pow(momentCentered.get(1, 2), 2) * featureMomentCentered.interaction(1, 2) -
185 pow(momentCentered.get(3, 0), 3) * featureMomentCentered.interaction(0, 3) -
186 3 * pow(momentCentered.get(2, 1), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(0, 3) +
187 3 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(0, 3) +
188 3 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(0, 3) -
189 6 * momentCentered.get(1, 2) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(0, 3) -
190 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(2, 1) *
191 featureMomentCentered.interaction(0, 3) +
192 2 * pow(momentCentered.get(1, 2), 3) * featureMomentCentered.interaction(0, 3);
193
194 LI[8] = -2 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) *
195 featureMomentCentered.interaction(3, 0) +
196 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(1, 2) *
197 featureMomentCentered.interaction(2, 1) -
198 6 * featureMomentCentered.interaction(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) *
199 momentCentered.get(1, 2) -
200 6 * momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) * momentCentered.get(0, 3) *
201 momentCentered.get(1, 2) -
202 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * featureMomentCentered.interaction(0, 3) *
203 momentCentered.get(1, 2) -
204 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) *
205 featureMomentCentered.interaction(1, 2) -
206 2 * momentCentered.get(3, 0) * momentCentered.get(1, 2) * momentCentered.get(0, 3) *
207 featureMomentCentered.interaction(0, 3) +
208 6 * momentCentered.get(2, 1) * momentCentered.get(1, 2) * momentCentered.get(0, 3) *
209 featureMomentCentered.interaction(1, 2) -
210 pow((double)momentCentered.get(3, 0), (double)3) * featureMomentCentered.interaction(1, 2) +
211 3 * featureMomentCentered.interaction(3, 0) * pow((double)momentCentered.get(1, 2), (double)3) +
212 6 * pow((double)momentCentered.get(2, 1), (double)3) * featureMomentCentered.interaction(0, 3) -
213 featureMomentCentered.interaction(2, 1) * pow((double)momentCentered.get(0, 3), (double)3) +
214 3 * featureMomentCentered.interaction(2, 1) * pow((double)momentCentered.get(1, 2), (double)2) *
215 momentCentered.get(0, 3) +
216 18 * pow((double)momentCentered.get(2, 1), (double)2) * momentCentered.get(0, 3) *
217 featureMomentCentered.interaction(2, 1) -
218 pow((double)momentCentered.get(3, 0), (double)2) * featureMomentCentered.interaction(2, 1) *
219 momentCentered.get(0, 3) +
220 9 * momentCentered.get(3, 0) * pow((double)momentCentered.get(1, 2), (double)2) *
221 featureMomentCentered.interaction(1, 2) -
222 4 * pow((double)momentCentered.get(3, 0), (double)2) * momentCentered.get(1, 2) *
223 featureMomentCentered.interaction(1, 2) +
224 2 * pow((double)momentCentered.get(1, 2), (double)2) * momentCentered.get(0, 3) *
225 featureMomentCentered.interaction(0, 3) -
226 4 * momentCentered.get(3, 0) * pow((double)momentCentered.get(1, 2), (double)2) *
227 featureMomentCentered.interaction(3, 0) +
228 2 * momentCentered.get(1, 2) * pow((double)momentCentered.get(0, 3), (double)2) *
229 featureMomentCentered.interaction(1, 2) -
230 4 * momentCentered.get(2, 1) * pow((double)momentCentered.get(0, 3), (double)2) *
231 featureMomentCentered.interaction(2, 1) +
232 3 * momentCentered.get(3, 0) * pow((double)momentCentered.get(2, 1), (double)2) *
233 featureMomentCentered.interaction(1, 2) -
234 3 * pow((double)momentCentered.get(3, 0), (double)2) * momentCentered.get(1, 2) *
235 featureMomentCentered.interaction(3, 0) -
236 momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) *
237 pow((double)momentCentered.get(0, 3), (double)2) -
238 4 * pow((double)momentCentered.get(2, 1), (double)2) * momentCentered.get(0, 3) *
239 featureMomentCentered.interaction(0, 3) -
240 3 * momentCentered.get(2, 1) * pow((double)momentCentered.get(0, 3), (double)2) *
241 featureMomentCentered.interaction(0, 3) +
242 2 * momentCentered.get(3, 0) * pow((double)momentCentered.get(2, 1), (double)2) *
243 featureMomentCentered.interaction(3, 0) +
244 2 * pow((double)momentCentered.get(3, 0), (double)2) * momentCentered.get(2, 1) *
245 featureMomentCentered.interaction(2, 1) +
246 3 * featureMomentCentered.interaction(3, 0) * pow((double)momentCentered.get(2, 1), (double)2) *
247 momentCentered.get(1, 2) -
248 pow((double)momentCentered.get(3, 0), (double)2) * momentCentered.get(2, 1) *
249 featureMomentCentered.interaction(0, 3) +
250 3 * momentCentered.get(2, 1) * pow((double)momentCentered.get(1, 2), (double)2) *
251 featureMomentCentered.interaction(0, 3) -
252 featureMomentCentered.interaction(3, 0) * momentCentered.get(1, 2) *
253 pow((double)momentCentered.get(0, 3), (double)2);
254
255 LI[9] = 4 * pow(momentCentered.get(3, 0), 3) * featureMomentCentered.interaction(3, 0) +
256 18 * momentCentered.get(1, 2) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(3, 0) +
257 12 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * momentCentered.get(3, 0) *
258 featureMomentCentered.interaction(3, 0) +
259 18 * pow(momentCentered.get(1, 2), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(3, 0) +
260 4 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(3, 0) +
261 18 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(2, 1) *
262 featureMomentCentered.interaction(3, 0) +
263 6 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(1, 2) * featureMomentCentered.interaction(3, 0) +
264 6 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(2, 1) +
265 18 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(3, 0) *
266 featureMomentCentered.interaction(2, 1) +
267 18 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(2, 1) +
268 6 * pow(momentCentered.get(0, 3), 3) * featureMomentCentered.interaction(2, 1) +
269 6 * pow(momentCentered.get(3, 0), 3) * featureMomentCentered.interaction(1, 2) +
270 18 * momentCentered.get(1, 2) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(1, 2) +
271 18 * momentCentered.get(0, 3) * momentCentered.get(2, 1) * momentCentered.get(3, 0) *
272 featureMomentCentered.interaction(1, 2) +
273 6 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) +
274 6 * momentCentered.get(2, 1) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(0, 3) +
275 4 * momentCentered.get(0, 3) * pow(momentCentered.get(3, 0), 2) * featureMomentCentered.interaction(0, 3) +
276 18 * momentCentered.get(1, 2) * momentCentered.get(2, 1) * momentCentered.get(3, 0) *
277 featureMomentCentered.interaction(0, 3) +
278 12 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(3, 0) *
279 featureMomentCentered.interaction(0, 3) +
280 18 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.interaction(0, 3) +
281 18 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(0, 3) +
282 4 * pow(momentCentered.get(0, 3), 3) * featureMomentCentered.interaction(0, 3);
283
284 LI[10] = featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) +
285 momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) -
286 4 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) -
287 4 * momentCentered.get(3, 1) * featureMomentCentered.interaction(1, 3) +
288 6 * momentCentered.get(2, 2) * featureMomentCentered.interaction(2, 2);
289
290 LI[11] = -3 * featureMomentCentered.interaction(4, 0) * momentCentered.get(2, 2) -
291 3 * momentCentered.get(4, 0) * featureMomentCentered.interaction(2, 2) -
292 2 * featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) -
293 2 * momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) +
294 6 * momentCentered.get(3, 1) * featureMomentCentered.interaction(3, 1) +
295 2 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) +
296 2 * momentCentered.get(3, 1) * featureMomentCentered.interaction(1, 3) -
297 3 * featureMomentCentered.interaction(2, 2) * momentCentered.get(0, 4) -
298 3 * momentCentered.get(2, 2) * featureMomentCentered.interaction(0, 4) +
299 6 * momentCentered.get(1, 3) * featureMomentCentered.interaction(1, 3);
300
301 LI[12] = 6 * momentCentered.get(4, 0) * featureMomentCentered.interaction(4, 0) +
302 12 * featureMomentCentered.interaction(4, 0) * momentCentered.get(2, 2) +
303 12 * momentCentered.get(4, 0) * featureMomentCentered.interaction(2, 2) +
304 2 * featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) +
305 2 * momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) +
306 16 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) +
307 16 * momentCentered.get(3, 1) * featureMomentCentered.interaction(1, 3) +
308 12 * featureMomentCentered.interaction(2, 2) * momentCentered.get(0, 4) +
309 12 * momentCentered.get(2, 2) * featureMomentCentered.interaction(0, 4) +
310 6 * momentCentered.get(0, 4) * featureMomentCentered.interaction(0, 4);
311
312 LI[13] = 2 * (momentCentered.get(5, 0) + 2 * momentCentered.get(3, 2) + momentCentered.get(1, 4)) *
313 (featureMomentCentered.interaction(5, 0) + 2 * featureMomentCentered.interaction(3, 2) +
314 featureMomentCentered.interaction(1, 4)) +
315 2 * (momentCentered.get(0, 5) + 2 * momentCentered.get(2, 3) + momentCentered.get(4, 1)) *
316 (featureMomentCentered.interaction(0, 5) + 2 * featureMomentCentered.interaction(2, 3) +
317 featureMomentCentered.interaction(4, 1));
318
319 LI[14] = 2 * (momentCentered.get(5, 0) - 2 * momentCentered.get(3, 2) - 3 * momentCentered.get(1, 4)) *
320 (featureMomentCentered.interaction(5, 0) - 2 * featureMomentCentered.interaction(3, 2) -
321 3 * featureMomentCentered.interaction(1, 4)) +
322 2 * (momentCentered.get(0, 5) - 2 * momentCentered.get(2, 3) - 3 * momentCentered.get(4, 1)) *
323 (featureMomentCentered.interaction(0, 5) - 2 * featureMomentCentered.interaction(2, 3) -
324 3 * featureMomentCentered.interaction(4, 1));
325
326 LI[15] = 2 * (momentCentered.get(5, 0) - 10 * momentCentered.get(3, 2) + 5 * momentCentered.get(1, 4)) *
327 (featureMomentCentered.interaction(5, 0) - 10 * featureMomentCentered.interaction(3, 2) +
328 5 * featureMomentCentered.interaction(1, 4)) +
329 2 * (momentCentered.get(0, 5) - 10 * momentCentered.get(2, 3) + 5 * momentCentered.get(4, 1)) *
330 (featureMomentCentered.interaction(0, 5) - 10 * featureMomentCentered.interaction(2, 3) +
331 5 * featureMomentCentered.interaction(4, 1));
332
333 double s3 = momentCInvariant.getS(3);
334 double s2 = momentCInvariant.getS(2);
335 double c3 = momentCInvariant.getC(3);
336 double c2 = momentCInvariant.getC(2);
337 double I1 = momentCInvariant.getII(1);
338 double I2 = momentCInvariant.getII(2);
339 double I3 = momentCInvariant.getII(3);
340
341 vpMatrix Lc2 = featureMomentCentered.interaction(0, 3) - 3 * featureMomentCentered.interaction(2, 1);
342 vpMatrix Ls2 = featureMomentCentered.interaction(3, 0) - 3 * featureMomentCentered.interaction(1, 2);
343 vpMatrix Lc3 = 2 * (momentCentered.get(2, 0) - momentCentered.get(0, 2)) *
344 (featureMomentCentered.interaction(2, 0) - featureMomentCentered.interaction(0, 2)) -
345 8 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
346 vpMatrix Ls3 = 4 * featureMomentCentered.interaction(1, 1) * (momentCentered.get(2, 0) - momentCentered.get(0, 2)) +
347 4 * momentCentered.get(1, 1) *
348 (featureMomentCentered.interaction(2, 0) - featureMomentCentered.interaction(0, 2));
349 vpMatrix LI1 = 2 * (momentCentered.get(2, 0) - momentCentered.get(0, 2)) *
350 (featureMomentCentered.interaction(2, 0) - featureMomentCentered.interaction(0, 2)) +
351 8 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
352 vpMatrix LI2 = 2 * (momentCentered.get(0, 3) - 3 * momentCentered.get(2, 1)) *
353 (featureMomentCentered.interaction(0, 3) - 3 * featureMomentCentered.interaction(2, 1)) +
354 2 * (momentCentered.get(3, 0) - 3 * momentCentered.get(1, 2)) *
355 (featureMomentCentered.interaction(3, 0) - 3 * featureMomentCentered.interaction(1, 2));
356 vpMatrix LI3 = featureMomentCentered.interaction(2, 0) + featureMomentCentered.interaction(0, 2);
357
358 vpMatrix La(1, 6);
359 double a;
360 if (momentObject.getType() == vpMomentObject::DISCRETE) {
361 a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
362 La = (featureMomentCentered.interaction(2, 0) + featureMomentCentered.interaction(0, 2));
363 } else {
364 a = momentObject.get(0, 0);
365 La = featureMomentBasic.interaction(0, 0);
366 }
368
369 interaction_matrices[0] = (1. / (momentCInvariant.getI(2) * momentCInvariant.getI(2))) *
370 (momentCInvariant.getI(2) * LI[1] - momentCInvariant.getI(1) * LI[2]);
371 interaction_matrices[1] = (1. / (momentCInvariant.getI(4) * momentCInvariant.getI(4))) *
372 (momentCInvariant.getI(4) * LI[3] - momentCInvariant.getI(3) * LI[4]);
373
374 interaction_matrices[2] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
375 (momentCInvariant.getI(6) * LI[5] - momentCInvariant.getI(5) * LI[6]);
376
377 interaction_matrices[3] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
378 (momentCInvariant.getI(6) * LI[7] - momentCInvariant.getI(7) * LI[6]);
379
380 interaction_matrices[4] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
381 (momentCInvariant.getI(6) * LI[8] - momentCInvariant.getI(8) * LI[6]);
382
383 interaction_matrices[5] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
384 (momentCInvariant.getI(6) * LI[9] - momentCInvariant.getI(9) * LI[6]);
385
386 interaction_matrices[6] = (1. / (momentCInvariant.getI(10) * momentCInvariant.getI(10))) *
387 (momentCInvariant.getI(10) * LI[11] - momentCInvariant.getI(11) * LI[10]);
388
389 interaction_matrices[7] = (1. / (momentCInvariant.getI(10) * momentCInvariant.getI(10))) *
390 (momentCInvariant.getI(10) * LI[12] - momentCInvariant.getI(12) * LI[10]);
391
392 interaction_matrices[8] = (1. / (momentCInvariant.getI(15) * momentCInvariant.getI(15))) *
393 (momentCInvariant.getI(15) * LI[13] - momentCInvariant.getI(13) * LI[15]);
394
395 interaction_matrices[9] = (1. / (momentCInvariant.getI(15) * momentCInvariant.getI(15))) *
396 (momentCInvariant.getI(15) * LI[14] - momentCInvariant.getI(14) * LI[15]);
397
398 interaction_matrices[10] = (Lc2 * c3 + c2 * Lc3 + Ls2 * s3 + s2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) +
399 (c2 * c3 + s2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 -
400 (c2 * c3 + s2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 -
401 0.3e1 / 0.2e1 * (c2 * c3 + s2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
402
403 interaction_matrices[11] = (Ls2 * c3 + s2 * Lc3 - Lc2 * s3 - c2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) +
404 (s2 * c3 - c2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 -
405 (s2 * c3 - c2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 -
406 0.3e1 / 0.2e1 * (s2 * c3 - c2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
407
408 interaction_matrices[12] = (1 / (I3 * I3)) * LI1 - (2 * I1 / (I3 * I3 * I3)) * LI3;
410 (I2 / (I3 * I3 * I3)) * La + (a / (I3 * I3 * I3)) * LI2 - (3 * a * I2 / (I3 * I3 * I3 * I3)) * LI3;
411}
412
413#else
414#include <visp3/core/vpMomentCInvariant.h>
415#include <visp3/core/vpMomentCentered.h>
416#include <visp3/visual_features/vpFeatureMomentBasic.h>
417#include <visp3/visual_features/vpFeatureMomentCInvariant.h>
418#include <visp3/visual_features/vpFeatureMomentCentered.h>
419
420#include <visp3/visual_features/vpFeatureMomentDatabase.h>
421
422#include <cmath>
423#include <iostream>
424#include <limits>
425#include <vector>
426
437{
438
439 // std::vector<vpMatrix> LI(16);
440 LI.resize(16); // LI made class member
441
442 bool found_moment_centered;
443 bool found_moment_cinvariant;
444 bool found_FeatureMoment_centered;
445 bool found_featuremoment_basic;
446
447 const vpMomentObject &momentObject = moment->getObject();
448 const vpMomentCentered &momentCentered =
449 (static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered)));
450 const vpMomentCInvariant &momentCInvariant =
451 (static_cast<const vpMomentCInvariant &>(moments.get("vpMomentCInvariant", found_moment_cinvariant)));
452
453 vpFeatureMomentCentered &featureMomentCentered = (static_cast<vpFeatureMomentCentered &>(
454 featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered)));
455
456 vpFeatureMomentBasic &featureMomentBasic = (static_cast<vpFeatureMomentBasic &>(
457 featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic)));
458
459 if (!found_featuremoment_basic)
460 throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found");
461 if (!found_moment_centered)
462 throw vpException(vpException::notInitialized, "vpMomentCentered not found");
463 if (!found_moment_cinvariant)
464 throw vpException(vpException::notInitialized, "vpMomentCInvariant not found");
465 if (!found_FeatureMoment_centered)
466 throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
467
468 vpMatrix zeros(1, 6);
469 for (int i = 0; i < 6; i++)
470 zeros[0][i] = 0;
471
472 double mu30 = momentCentered.get(3, 0);
473 double mu30_2 = mu30 * mu30;
474 double mu30_3 = mu30_2 * mu30;
475
476 double mu03 = momentCentered.get(0, 3);
477 double mu03_2 = mu03 * mu03;
478 double mu03_3 = mu03 * mu03_2;
479
480 double mu20 = momentCentered.get(2, 0);
481 double mu02 = momentCentered.get(0, 2);
482 double mu50 = momentCentered.get(5, 0);
483 double mu32 = momentCentered.get(3, 2);
484 double mu14 = momentCentered.get(1, 4);
485 double mu05 = momentCentered.get(0, 5);
486 double mu23 = momentCentered.get(2, 3);
487 double mu41 = momentCentered.get(4, 1);
488 double mu40 = momentCentered.get(4, 0);
489 double mu04 = momentCentered.get(0, 4);
490 double mu31 = momentCentered.get(3, 1);
491 double mu13 = momentCentered.get(1, 3);
492 double mu22 = momentCentered.get(2, 2);
493 double mu21 = momentCentered.get(2, 1);
494 double mu12 = momentCentered.get(1, 2);
495 double mu11 = momentCentered.get(1, 1);
496
497 double mu12_2 = mu12 * mu12;
498 double mu21_2 = mu21 * mu21;
499 double mu21_3 = mu21 * mu21_2;
500 double mu12_3 = mu12_2 * mu12;
501
502 vpMatrix Lmu20 = featureMomentCentered.interaction(2, 0);
503 vpMatrix Lmu02 = featureMomentCentered.interaction(0, 2);
504 vpMatrix Lmu11 = featureMomentCentered.interaction(1, 1);
505 vpMatrix Lmu12 = featureMomentCentered.interaction(1, 2);
506 vpMatrix Lmu03 = featureMomentCentered.interaction(0, 3);
507 vpMatrix Lmu21 = featureMomentCentered.interaction(2, 1);
508 vpMatrix Lmu30 = featureMomentCentered.interaction(3, 0);
509 vpMatrix Lmu40 = featureMomentCentered.interaction(4, 0);
510 vpMatrix Lmu04 = featureMomentCentered.interaction(0, 4);
511 vpMatrix Lmu31 = featureMomentCentered.interaction(3, 1);
512 vpMatrix Lmu13 = featureMomentCentered.interaction(1, 3);
513 vpMatrix Lmu22 = featureMomentCentered.interaction(2, 2);
514 vpMatrix Lmu05 = featureMomentCentered.interaction(0, 5);
515 vpMatrix Lmu14 = featureMomentCentered.interaction(1, 4);
516 vpMatrix Lmu23 = featureMomentCentered.interaction(2, 3);
517 vpMatrix Lmu32 = featureMomentCentered.interaction(3, 2);
518 vpMatrix Lmu41 = featureMomentCentered.interaction(4, 1);
519 vpMatrix Lmu50 = featureMomentCentered.interaction(5, 0);
520
521 LI[1] = -Lmu20 * mu02 - mu20 * Lmu02 + 2 * mu11 * Lmu11;
522
523 LI[2] = (-2 * mu20 + 2 * mu02) * Lmu02 + 8 * mu11 * Lmu11 + (2 * mu20 - 2 * mu02) * Lmu20;
524
525 LI[3] = (-6 * mu21 + 2 * mu03) * Lmu03 + (-6 * mu30 + 18 * mu12) * Lmu12 + (18 * mu21 - 6 * mu03) * Lmu21 +
526 (2 * mu30 - 6 * mu12) * Lmu30;
527
528 LI[4] = (2 * mu21 + 2 * mu03) * Lmu03 + (2 * mu30 + 2 * mu12) * Lmu12 + (2 * mu21 + 2 * mu03) * Lmu21 +
529 (2 * mu30 + 2 * mu12) * Lmu30;
530
531 LI[5] = (-2 * mu30_2 * mu03 + 6 * mu30 * mu21 * mu12 - 4 * mu21_3) * Lmu03 +
532 (6 * mu30 * mu21 * mu03 - 12 * mu30 * mu12_2 + 6 * mu21_2 * mu12) * Lmu12 +
533 (6 * mu30 * mu12 * mu03 - 12 * mu21_2 * mu03 + 6 * mu21 * mu12_2) * Lmu21 +
534 (-2 * mu30 * mu03_2 - 4 * mu12_3 + 6 * mu21 * mu12 * mu03) * Lmu30;
535
536 LI[6] = (-6 * mu30 * mu21 * mu12 - 6 * mu21 * mu12_2 + 6 * mu21_2 * mu03 + 2 * mu21_3 + 4 * mu30_2 * mu03) * Lmu03 +
537 (-6 * mu30 * mu21_2 - 6 * mu30 * mu21 * mu03 + 12 * mu12_3 + 6 * mu30_2 * mu12 - 12 * mu21 * mu12 * mu03 +
538 6 * mu30 * mu12_2) *
539 Lmu12 +
540 (6 * mu21 * mu03_2 + 6 * mu21_2 * mu03 - 6 * mu30 * mu12 * mu03 + 12 * mu21_3 - 12 * mu30 * mu21 * mu12 -
541 6 * mu12_2 * mu03) *
542 Lmu21 +
543 (6 * mu30 * mu12_2 + 2 * mu12_3 + 4 * mu30 * mu03_2 - 6 * mu21_2 * mu12 - 6 * mu21 * mu12 * mu03) * Lmu30;
544
545 LI[7] = (-6 * mu21_2 * mu12 + 3 * mu30 * mu03_2 - mu30_3 - 3 * mu30 * mu21_2 - 6 * mu21 * mu12 * mu03 +
546 3 * mu30 * mu12_2 + 2 * mu12_3) *
547 Lmu03 +
548 (-3 * mu21 * mu03_2 + 12 * mu30 * mu21 * mu12 + 6 * mu30 * mu12 * mu03 + 3 * mu30_2 * mu21 +
549 9 * mu21 * mu12_2 - 6 * mu21_2 * mu03 - 3 * mu21_3 + 6 * mu12_2 * mu03) *
550 Lmu12 +
551 (3 * mu30_2 * mu12 - 9 * mu21_2 * mu12 - 12 * mu21 * mu12 * mu03 - 6 * mu30 * mu21 * mu03 -
552 6 * mu30 * mu21_2 + 6 * mu30 * mu12_2 + 3 * mu12_3 - 3 * mu12 * mu03_2) *
553 Lmu21 +
554 (6 * mu21 * mu12_2 + 6 * mu30 * mu21 * mu12 - 3 * mu30_2 * mu03 + 3 * mu12_2 * mu03 - 3 * mu21_2 * mu03 -
555 2 * mu21_3 + mu03_3) *
556 Lmu30;
557
558 LI[8] = (6 * mu21_3 - 2 * mu30 * mu12 * mu03 + 2 * mu12_2 * mu03 + 3 * mu21 * mu12_2 - 6 * mu30 * mu21 * mu12 -
559 mu30_2 * mu21 - 4 * mu21_2 * mu03 - 3 * mu21 * mu03_2) *
560 Lmu03 +
561 (2 * mu12 * mu03_2 - 4 * mu30_2 * mu12 + 9 * mu30 * mu12_2 - mu30 * mu03_2 - 6 * mu30 * mu21 * mu03 +
562 3 * mu30 * mu21_2 + 6 * mu21 * mu12 * mu03 - mu30_3) *
563 Lmu12 +
564 (18 * mu21_2 * mu03 + 6 * mu30 * mu21 * mu12 - 4 * mu21 * mu03_2 - mu03_3 - mu30_2 * mu03 -
565 6 * mu30 * mu12 * mu03 + 3 * mu12_2 * mu03 + 2 * mu30_2 * mu21) *
566 Lmu21 +
567 (-6 * mu21 * mu12 * mu03 - 4 * mu30 * mu12_2 - 2 * mu30 * mu21 * mu03 + 2 * mu30 * mu21_2 + 3 * mu12_3 +
568 3 * mu21_2 * mu12 - 3 * mu30_2 * mu12 - mu12 * mu03_2) *
569 Lmu30;
570
571 LI[9] = (2 * (2 * mu03 + 3 * mu21)) * (3 * mu03 * mu21 + 3 * mu30 * mu12 + mu30_2 + mu03_2) * Lmu03 +
572 6 * mu30 * (3 * mu03 * mu21 + 3 * mu30 * mu12 + mu30_2 + mu03_2) * Lmu12 +
573 6 * mu03 * (3 * mu03 * mu21 + 3 * mu30 * mu12 + mu30_2 + mu03_2) * Lmu21 +
574 (2 * (2 * mu30 + 3 * mu12)) * (3 * mu03 * mu21 + 3 * mu30 * mu12 + mu30_2 + mu03_2) * Lmu30;
575
576 LI[10] = Lmu40 * mu04 + mu40 * Lmu04 - 4 * Lmu31 * mu13 - 4 * mu31 * Lmu13 + 6 * mu22 * Lmu22;
577
578 LI[11] = (-2 * mu40 - 3 * mu22) * Lmu04 + (2 * mu31 + 6 * mu13) * Lmu13 + (-3 * mu04 - 3 * mu40) * Lmu22 +
579 (2 * mu13 + 6 * mu31) * Lmu31 + (-3 * mu22 - 2 * mu04) * Lmu40;
580
581 LI[12] = (2 * mu40 + 12 * mu22 + 6 * mu04) * Lmu04 + 16 * mu31 * Lmu13 + (12 * mu40 + 12 * mu04) * Lmu22 +
582 16 * Lmu31 * mu13 + (6 * mu40 + 12 * mu22 + 2 * mu04) * Lmu40;
583
584 LI[13] = (2 * mu05 + 4 * mu23 + 2 * mu41) * Lmu05 + (2 * mu50 + 4 * mu32 + 2 * mu14) * Lmu14 +
585 (4 * mu05 + 8 * mu23 + 4 * mu41) * Lmu23 + (4 * mu50 + 8 * mu32 + 4 * mu14) * Lmu32 +
586 (2 * mu05 + 4 * mu23 + 2 * mu41) * Lmu41 + (2 * mu50 + 4 * mu32 + 2 * mu14) * Lmu50;
587
588 LI[14] = (2 * mu05 - 4 * mu23 - 6 * mu41) * Lmu05 + (-6 * mu50 + 12 * mu32 + 18 * mu14) * Lmu14 +
589 (-4 * mu05 + 8 * mu23 + 12 * mu41) * Lmu23 + (-4 * mu50 + 8 * mu32 + 12 * mu14) * Lmu32 +
590 (-6 * mu05 + 12 * mu23 + 18 * mu41) * Lmu41 + (2 * mu50 - 4 * mu32 - 6 * mu14) * Lmu50;
591
592 LI[15] = (2 * mu05 - 20 * mu23 + 10 * mu41) * Lmu05 + (10 * mu50 - 100 * mu32 + 50 * mu14) * Lmu14 +
593 (-20 * mu05 + 200 * mu23 - 100 * mu41) * Lmu23 + (-20 * mu50 + 200 * mu32 - 100 * mu14) * Lmu32 +
594 (10 * mu05 - 100 * mu23 + 50 * mu41) * Lmu41 + (2 * mu50 - 20 * mu32 + 10 * mu14) * Lmu50;
595
596 double s3 = momentCInvariant.getS(3);
597 double s2 = momentCInvariant.getS(2);
598 double c3 = momentCInvariant.getC(3);
599 double c2 = momentCInvariant.getC(2);
600
601 double I1 = momentCInvariant.getII(1);
602 double I2 = momentCInvariant.getII(2);
603 double I3 = momentCInvariant.getII(3);
604
605 vpMatrix Lmu20__Lmu02 = Lmu20 - Lmu02;
606 double mu20__mu02 = mu20 - mu02;
607 vpMatrix Lc2 = Lmu03 - 3 * Lmu21;
608 vpMatrix Ls2 = Lmu30 - 3 * Lmu12;
609 vpMatrix Lc3 = 2 * (mu20__mu02) * (Lmu20__Lmu02)-8. * mu11 * Lmu11;
610 vpMatrix Ls3 = 4 * Lmu11 * (mu20__mu02) + 4 * mu11 * (Lmu20__Lmu02);
611 vpMatrix LI1 = 2 * (mu20__mu02) * (Lmu20__Lmu02) + 8 * mu11 * Lmu11;
612 vpMatrix LI2 = 2 * (mu03 - 3 * mu21) * (Lc2) + 2 * (mu30 - 3 * mu12) * (Ls2);
613 vpMatrix LI3 = Lmu20 + Lmu02;
614
615 vpMatrix La(1, 6);
616 double a;
617 if (momentObject.getType() == vpMomentObject::DISCRETE) {
618 a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
619 La = (featureMomentCentered.interaction(2, 0) + featureMomentCentered.interaction(0, 2));
620 } else {
621 a = momentObject.get(0, 0);
622 La = featureMomentBasic.interaction(0, 0);
623 }
624
626
627 /*
628 momentCInvariant.printInvariants(std::cout);
629 printLsofInvariants(std::cout);
630 */
631
632 interaction_matrices[0] = (1. / (momentCInvariant.getI(2) * momentCInvariant.getI(2))) *
633 (momentCInvariant.getI(2) * LI[1] - momentCInvariant.getI(1) * LI[2]);
634
635 interaction_matrices[1] = (1. / (momentCInvariant.getI(4) * momentCInvariant.getI(4))) *
636 (momentCInvariant.getI(4) * LI[3] - momentCInvariant.getI(3) * LI[4]);
637
638 interaction_matrices[2] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
639 (momentCInvariant.getI(6) * LI[5] - momentCInvariant.getI(5) * LI[6]);
640
641 interaction_matrices[3] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
642 (momentCInvariant.getI(6) * LI[7] - momentCInvariant.getI(7) * LI[6]);
643
644 interaction_matrices[4] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
645 (momentCInvariant.getI(6) * LI[8] - momentCInvariant.getI(8) * LI[6]);
646
647 interaction_matrices[5] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
648 (momentCInvariant.getI(6) * LI[9] - momentCInvariant.getI(9) * LI[6]);
649
650 interaction_matrices[6] = (1. / (momentCInvariant.getI(10) * momentCInvariant.getI(10))) *
651 (momentCInvariant.getI(10) * LI[11] - momentCInvariant.getI(11) * LI[10]);
652
653 interaction_matrices[7] = (1. / (momentCInvariant.getI(10) * momentCInvariant.getI(10))) *
654 (momentCInvariant.getI(10) * LI[12] - momentCInvariant.getI(12) * LI[10]);
655
656 interaction_matrices[8] = (1. / (momentCInvariant.getI(15) * momentCInvariant.getI(15))) *
657 (momentCInvariant.getI(15) * LI[13] - momentCInvariant.getI(13) * LI[15]);
658
659 interaction_matrices[9] = (1. / (momentCInvariant.getI(15) * momentCInvariant.getI(15))) *
660 (momentCInvariant.getI(15) * LI[14] - momentCInvariant.getI(14) * LI[15]);
661
662 interaction_matrices[10] = (Lc2 * c3 + c2 * Lc3 + Ls2 * s3 + s2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) +
663 (c2 * c3 + s2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 -
664 (c2 * c3 + s2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 -
665 0.3e1 / 0.2e1 * (c2 * c3 + s2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
666
667 interaction_matrices[11] = (Ls2 * c3 + s2 * Lc3 - Lc2 * s3 - c2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) +
668 (s2 * c3 - c2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 -
669 (s2 * c3 - c2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 -
670 0.3e1 / 0.2e1 * (s2 * c3 - c2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
671
672 interaction_matrices[12] = (1 / (I3 * I3)) * LI1 - (2 * I1 / (I3 * I3 * I3)) * LI3;
673
675 (I2 / (I3 * I3 * I3)) * La + (a / (I3 * I3 * I3)) * LI2 - (3 * a * I2 / (I3 * I3 * I3 * I3)) * LI3;
676}
677
684{
685 for (unsigned int i = 1; i < 15; ++i) {
686 os << "LI[" << i << "] = ";
687 LI[i].matlabPrint(os);
688 os << std::endl;
689 }
690}
691
696std::ostream &operator<<(std::ostream &os, const vpFeatureMomentCInvariant &featcinv)
697{
698 // Print L for c1 .. c10
699 for (unsigned int i = 0; i < 10; ++i) {
700 os << "L_c[" << i << "] = ";
701 featcinv.interaction_matrices[i].matlabPrint(os);
702 os << std::endl;
703 }
704
705 // sx, sy
706 os << "L_sx = ";
707 featcinv.interaction_matrices[10].matlabPrint(os);
708 os << std::endl;
709 os << "L_sy = ";
710 featcinv.interaction_matrices[11].matlabPrint(os);
711 os << std::endl;
712 // Px,Py
713 os << "L_Px = ";
714 featcinv.interaction_matrices[12].matlabPrint(os);
715 os << std::endl;
716 os << "L_Py = ";
717 featcinv.interaction_matrices[13].matlabPrint(os);
718 os << std::endl;
719
720 return os;
721}
722#endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition vpException.h:86
Functionality computation for basic moment feature. Computes the interaction matrix associated with v...
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
Functionality computation for 2D rotation/translation/scale non-symmetric invariant moment feature....
void printLsofInvariants(std::ostream &os) const
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
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
double getI(unsigned int index) const
double getS(unsigned int i) const
double getC(unsigned int i) const
double getII(unsigned int i) const
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 for generic objects.
const std::vector< double > & get() const
vpObjectType getType() const
const vpMomentObject & getObject() const
Definition vpMoment.h:145