Visual Servoing Platform  version 3.1.0
vpFeatureMomentCInvariant.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Implementation for all supported moment features.
33  *
34  * Authors:
35  * Filip Novotny
36  *
37  *****************************************************************************/
38 #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 
60 void vpFeatureMomentCInvariant::compute_interaction()
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  }
367  interaction_matrices.resize(14);
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;
409  interaction_matrices[13] =
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 
436 void vpFeatureMomentCInvariant::compute_interaction()
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 
625  interaction_matrices.resize(14);
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 
674  interaction_matrices[13] =
675  (I2 / (I3 * I3 * I3)) * La + (a / (I3 * I3 * I3)) * LI2 - (3 * a * I2 / (I3 * I3 * I3 * I3)) * LI3;
676 
677  /*
678  std::cout << (*this);
679  vpTRACE("Done.");
680  std::exit(-1);
681  */
682 }
683 
689 void vpFeatureMomentCInvariant::printLsofInvariants(std::ostream &os) const
690 {
691  for (unsigned int i = 1; i < 15; ++i) {
692  os << "LI[" << i << "] = ";
693  LI[i].matlabPrint(os);
694  os << std::endl;
695  }
696 }
697 
702 std::ostream &operator<<(std::ostream &os, const vpFeatureMomentCInvariant &featcinv)
703 {
704  // Print L for c1 .. c10
705  for (unsigned int i = 0; i < 10; ++i) {
706  os << "L_c[" << i << "] = ";
707  featcinv.interaction_matrices[i].matlabPrint(os);
708  os << std::endl;
709  }
710 
711  // sx, sy
712  os << "L_sx = ";
713  featcinv.interaction_matrices[10].matlabPrint(os);
714  os << std::endl;
715  os << "L_sy = ";
716  featcinv.interaction_matrices[11].matlabPrint(os);
717  os << std::endl;
718  // Px,Py
719  os << "L_Px = ";
720  featcinv.interaction_matrices[12].matlabPrint(os);
721  os << std::endl;
722  os << "L_Py = ";
723  featcinv.interaction_matrices[13].matlabPrint(os);
724  os << std::endl;
725 
726  return os;
727 }
728 #endif
vpFeatureMomentBasic
Functionality computation for basic moment feature. Computes the interaction matrix associated with v...
Definition: vpFeatureMomentBasic.h:78
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
vpMomentCentered::get
double get(unsigned int i, unsigned int j) const
Definition: vpMomentCentered.cpp:103
vpMomentCInvariant::getI
double getI(unsigned int index) const
Definition: vpMomentCInvariant.h:208
vpFeatureMoment::moment
const vpMoment * moment
Definition: vpFeatureMoment.h:162
vpFeatureMomentCentered
Functionality computation for centered moment feature. Computes the interaction matrix associated wit...
Definition: vpFeatureMomentCentered.h:76
vpMomentDatabase::get
const vpMoment & get(const char *type, bool &found) const
Definition: vpMomentDatabase.cpp:64
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpMomentObject
Class for generic objects.
Definition: vpMomentObject.h:219
vpFeatureMoment::interaction_matrices
std::vector< vpMatrix > interaction_matrices
Definition: vpFeatureMoment.h:166
vpMoment::getObject
const vpMomentObject & getObject() const
Definition: vpMoment.h:150
vpFeatureMomentCInvariant::printLsofInvariants
void printLsofInvariants(std::ostream &os) const
Definition: vpFeatureMomentCInvariant.cpp:689
vpFeatureMoment::moments
vpMomentDatabase & moments
Definition: vpFeatureMoment.h:164
vpMomentCInvariant::getII
double getII(unsigned int i) const
Definition: vpMomentCInvariant.h:218
vpMomentObject::get
const std::vector< double > & get() const
Definition: vpMomentObject.cpp:532
vpMomentCentered
This class defines the double-indexed centered moment descriptor .
Definition: vpMomentCentered.h:79
vpMomentObject::getType
vpObjectType getType() const
Definition: vpMomentObject.h:271
vpMomentCInvariant::getC
double getC(unsigned int i) const
Definition: vpMomentCInvariant.h:204
vpFeatureMoment::featureMomentsDataBase
vpFeatureMomentDatabase * featureMomentsDataBase
Definition: vpFeatureMoment.h:165
vpFeatureMomentCInvariant
Functionality computation for 2D rotation/translation/scale non-symmetric invariant moment feature...
Definition: vpFeatureMomentCInvariant.h:230
vpFeatureMomentBasic::interaction
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
Definition: vpFeatureMomentBasic.cpp:159
vpFeatureMomentCentered::interaction
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
Definition: vpFeatureMomentCentered.cpp:73
vpFeatureMomentCInvariant::operator<<
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpFeatureMomentCInvariant &featcinv)
Definition: vpFeatureMomentCInvariant.cpp:702
vpFeatureMomentDatabase::get
vpFeatureMoment & get(const char *type, bool &found)
Definition: vpFeatureMomentDatabase.cpp:64
vpMomentCInvariant::getS
double getS(unsigned int i) const
Definition: vpMomentCInvariant.h:227