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