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