ViSP  2.8.0
vpFeatureMomentCInvariant.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureMomentImpl.cpp 3317 2011-09-06 14:14:47Z fnovotny $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Implementation for all supported moment features.
36  *
37  * Authors:
38  * Filip Novotny
39  *
40  *****************************************************************************/
41 #include <visp/vpMomentObject.h>
42 #ifdef VISP_MOMENTS_COMBINE_MATRICES
43 #include <visp/vpFeatureMomentCInvariant.h>
44 #include <visp/vpMomentCentered.h>
45 #include <visp/vpMomentCInvariant.h>
46 #include <visp/vpFeatureMomentCentered.h>
47 #include <visp/vpFeatureMomentBasic.h>
48 
49 #include <visp/vpFeatureMomentDatabase.h>
50 
51 #include <vector>
52 #include <limits>
53 
54 
65  std::vector<vpMatrix> LI(16);
66  bool found_moment_centered;
67  bool found_moment_cinvariant;
68  bool found_FeatureMoment_centered;
69  bool found_featuremoment_basic;
70 
71  vpMomentObject& momentObject = moment->getObject();
72  vpMomentCentered& momentCentered = (static_cast<vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
73  vpMomentCInvariant& momentCInvariant = (static_cast<vpMomentCInvariant&>(moments.get("vpMomentCInvariant",found_moment_cinvariant)));
74  vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMoments->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
75 
76  vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMoments->get("vpFeatureMomentBasic",found_featuremoment_basic)));
77 
78  if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
79 
80 
81  if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
82  if(!found_moment_cinvariant) throw vpException(vpException::notInitialized,"vpMomentCInvariant not found");
83  if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
84 
85  vpMatrix zeros(1,6);
86  for(int i=0;i<6;i++) zeros[0][i]=0;
87 
88 
89  LI[1]=-featureMomentCentered.interaction(2, 0) * momentCentered.get(0, 2) - momentCentered.get(2, 0) * featureMomentCentered.interaction(0, 2) + 2 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
90 
91 
92  LI[2]=2 * (momentCentered.get(2, 0) - momentCentered.get(0, 2)) * (featureMomentCentered.interaction(2, 0) - featureMomentCentered.interaction(0, 2)) + 8 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
93 
94 
95  LI[3]=2 * (momentCentered.get(3, 0) - 3 * momentCentered.get(1, 2)) * (featureMomentCentered.interaction(3, 0) - 3 * featureMomentCentered.interaction(1, 2)) + 2 * (3 * momentCentered.get(2, 1) - momentCentered.get(0, 3)) * (3 * featureMomentCentered.interaction(2, 1) - featureMomentCentered.interaction(0, 3));
96 
97 
98  LI[4]=2 * (momentCentered.get(3, 0) + momentCentered.get(1, 2)) * (featureMomentCentered.interaction(3, 0) + featureMomentCentered.interaction(1, 2)) + 2 * (momentCentered.get(2, 1) + momentCentered.get(0, 3)) * (featureMomentCentered.interaction(2, 1) + featureMomentCentered.interaction(0, 3));
99 
100  LI[5]=-2*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)-4*pow(momentCentered.get(1,2),3)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(3,0)*featureMomentCentered.interaction(2,1)-12*momentCentered.get(0,3)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(2,1)+6*pow(momentCentered.get(1,2),2)*momentCentered.get(2,1)*featureMomentCentered.interaction(2,1)+6*momentCentered.get(0,3)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(1,2)-12*pow(momentCentered.get(1,2),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(1,2)+6*momentCentered.get(1,2)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(1,2)-2*momentCentered.get(0,3)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(0,3)+6*momentCentered.get(1,2)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(0,3)-4*pow(momentCentered.get(2,1),3)*featureMomentCentered.interaction(0,3);
101 
102 
103  LI[6]=6*pow(momentCentered.get(1,2),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+4*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)-6*momentCentered.get(1,2)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(3,0)-6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)+2*pow(momentCentered.get(1,2),3)*featureMomentCentered.interaction(3,0)-12*momentCentered.get(1,2)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(2,1)-6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(3,0)*featureMomentCentered.interaction(2,1)+12*pow(momentCentered.get(2,1),3)*featureMomentCentered.interaction(2,1)+6*momentCentered.get(0,3)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(2,1)+6*pow(momentCentered.get(0,3),2)*momentCentered.get(2,1)*featureMomentCentered.interaction(2,1)-6*momentCentered.get(0,3)*pow(momentCentered.get(1,2),2)*featureMomentCentered.interaction(2,1)+6*momentCentered.get(1,2)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(1,2)-6*pow(momentCentered.get(2,1),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(1,2)-6*momentCentered.get(0,3)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(1,2)+6*pow(momentCentered.get(1,2),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(1,2)-12*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(2,1)*featureMomentCentered.interaction(1,2)+12*pow(momentCentered.get(1,2),3)*featureMomentCentered.interaction(1,2)+4*momentCentered.get(0,3)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(0,3)-6*momentCentered.get(1,2)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(0,3)+2*pow(momentCentered.get(2,1),3)*featureMomentCentered.interaction(0,3)+6*momentCentered.get(0,3)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(0,3)-6*pow(momentCentered.get(1,2),2)*momentCentered.get(2,1)*featureMomentCentered.interaction(0,3);
104 
105 
106  LI[7] = -3*momentCentered.get(0,3)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(1,2)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)-2*pow(momentCentered.get(2,1),3)*featureMomentCentered.interaction(3,0)-3*momentCentered.get(0,3)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(3,0)+6*pow(momentCentered.get(1,2),2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)+3*momentCentered.get(0,3)*pow(momentCentered.get(1,2),2)*featureMomentCentered.interaction(3,0)+pow(momentCentered.get(0,3),3)*featureMomentCentered.interaction(3,0)+3*momentCentered.get(1,2)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(2,1)-6*pow(momentCentered.get(2,1),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(2,1)-6*momentCentered.get(0,3)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(2,1)+6*pow(momentCentered.get(1,2),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(2,1)-9*momentCentered.get(1,2)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(2,1)-12*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(2,1)*featureMomentCentered.interaction(2,1)+3*pow(momentCentered.get(1,2),3)*featureMomentCentered.interaction(2,1)-3*pow(momentCentered.get(0,3),2)*momentCentered.get(1,2)*featureMomentCentered.interaction(2,1)+3*momentCentered.get(2,1)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(1,2)+12*momentCentered.get(1,2)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(1,2)+6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(3,0)*featureMomentCentered.interaction(1,2)-3*pow(momentCentered.get(2,1),3)*featureMomentCentered.interaction(1,2)-6*momentCentered.get(0,3)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(1,2)+9*pow(momentCentered.get(1,2),2)*momentCentered.get(2,1)*featureMomentCentered.interaction(1,2)-3*pow(momentCentered.get(0,3),2)*momentCentered.get(2,1)*featureMomentCentered.interaction(1,2)+6*momentCentered.get(0,3)*pow(momentCentered.get(1,2),2)*featureMomentCentered.interaction(1,2)-pow(momentCentered.get(3,0),3)*featureMomentCentered.interaction(0,3)-3*pow(momentCentered.get(2,1),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(0,3)+3*pow(momentCentered.get(1,2),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(0,3)+3*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(0,3)-6*momentCentered.get(1,2)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(0,3)-6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(2,1)*featureMomentCentered.interaction(0,3)+2*pow(momentCentered.get(1,2),3)*featureMomentCentered.interaction(0,3);
107 
108  LI[8]=-2 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) * featureMomentCentered.interaction(3, 0) + 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(1, 2) * featureMomentCentered.interaction(2, 1) - 6 * featureMomentCentered.interaction(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) * momentCentered.get(1, 2) - 6 * momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) * momentCentered.get(0, 3) * momentCentered.get(1, 2) - 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * featureMomentCentered.interaction(0, 3) * momentCentered.get(1, 2) - 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) * featureMomentCentered.interaction(1, 2) - 2 * momentCentered.get(3, 0) * momentCentered.get(1, 2) * momentCentered.get(0, 3) * featureMomentCentered.interaction(0, 3) + 6 * momentCentered.get(2, 1) * momentCentered.get(1, 2) * momentCentered.get(0, 3) * featureMomentCentered.interaction(1, 2) - pow((double) momentCentered.get(3, 0), (double) 3) * featureMomentCentered.interaction(1, 2) + 3 * featureMomentCentered.interaction(3, 0) * pow((double) momentCentered.get(1, 2), (double) 3) + 6 * pow((double) momentCentered.get(2, 1), (double) 3) * featureMomentCentered.interaction(0, 3) - featureMomentCentered.interaction(2, 1) * pow((double) momentCentered.get(0, 3), (double) 3) + 3 * featureMomentCentered.interaction(2, 1) * pow((double) momentCentered.get(1, 2), (double) 2) * momentCentered.get(0, 3) + 18 * pow((double) momentCentered.get(2, 1), (double) 2) * momentCentered.get(0, 3) * featureMomentCentered.interaction(2, 1) - pow((double) momentCentered.get(3, 0), (double) 2) * featureMomentCentered.interaction(2, 1) * momentCentered.get(0, 3) + 9 * momentCentered.get(3, 0) * pow((double) momentCentered.get(1, 2), (double) 2) * featureMomentCentered.interaction(1, 2) - 4 * pow((double) momentCentered.get(3, 0), (double) 2) * momentCentered.get(1, 2) * featureMomentCentered.interaction(1, 2) + 2 * pow((double) momentCentered.get(1, 2), (double) 2) * momentCentered.get(0, 3) * featureMomentCentered.interaction(0, 3) - 4 * momentCentered.get(3, 0) * pow((double) momentCentered.get(1, 2), (double) 2) * featureMomentCentered.interaction(3, 0) + 2 * momentCentered.get(1, 2) * pow((double) momentCentered.get(0, 3), (double) 2) * featureMomentCentered.interaction(1, 2) - 4 * momentCentered.get(2, 1) * pow((double) momentCentered.get(0, 3), (double) 2) * featureMomentCentered.interaction(2, 1) + 3 * momentCentered.get(3, 0) * pow((double) momentCentered.get(2, 1), (double) 2) * featureMomentCentered.interaction(1, 2) - 3 * pow((double) momentCentered.get(3, 0), (double) 2) * momentCentered.get(1, 2) * featureMomentCentered.interaction(3, 0) - momentCentered.get(3, 0) * featureMomentCentered.interaction(1, 2) * pow((double) momentCentered.get(0, 3), (double) 2) - 4 * pow((double) momentCentered.get(2, 1), (double) 2) * momentCentered.get(0, 3) * featureMomentCentered.interaction(0, 3) - 3 * momentCentered.get(2, 1) * pow((double) momentCentered.get(0, 3), (double) 2) * featureMomentCentered.interaction(0, 3) + 2 * momentCentered.get(3, 0) * pow((double) momentCentered.get(2, 1), (double) 2) * featureMomentCentered.interaction(3, 0) + 2 * pow((double) momentCentered.get(3, 0), (double) 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(2, 1) + 3 * featureMomentCentered.interaction(3, 0) * pow((double) momentCentered.get(2, 1), (double) 2) * momentCentered.get(1, 2) - pow((double) momentCentered.get(3, 0), (double) 2) * momentCentered.get(2, 1) * featureMomentCentered.interaction(0, 3) + 3 * momentCentered.get(2, 1) * pow((double) momentCentered.get(1, 2), (double) 2) * featureMomentCentered.interaction(0, 3) - featureMomentCentered.interaction(3, 0) * momentCentered.get(1, 2) * pow((double) momentCentered.get(0, 3), (double) 2);
109 
110  LI[9] = 4*pow(momentCentered.get(3,0),3)*featureMomentCentered.interaction(3,0)+18*momentCentered.get(1,2)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(3,0)+12*momentCentered.get(0,3)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+18*pow(momentCentered.get(1,2),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+4*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+18*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)+6*pow(momentCentered.get(0,3),2)*momentCentered.get(1,2)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(0,3)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(2,1)+18*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(3,0)*featureMomentCentered.interaction(2,1)+18*pow(momentCentered.get(0,3),2)*momentCentered.get(2,1)*featureMomentCentered.interaction(2,1)+6*pow(momentCentered.get(0,3),3)*featureMomentCentered.interaction(2,1)+6*pow(momentCentered.get(3,0),3)*featureMomentCentered.interaction(1,2)+18*momentCentered.get(1,2)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(1,2)+18*momentCentered.get(0,3)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(1,2)+6*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(1,2)+6*momentCentered.get(2,1)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(0,3)+4*momentCentered.get(0,3)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(0,3)+18*momentCentered.get(1,2)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(0,3)+12*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(3,0)*featureMomentCentered.interaction(0,3)+18*momentCentered.get(0,3)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(0,3)+18*pow(momentCentered.get(0,3),2)*momentCentered.get(2,1)*featureMomentCentered.interaction(0,3)+4*pow(momentCentered.get(0,3),3)*featureMomentCentered.interaction(0,3);
111 
112 
113  LI[10]=featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) + momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) - 4 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) - 4 * momentCentered.get(3, 1) * featureMomentCentered.interaction(1, 3) + 6 * momentCentered.get(2, 2) * featureMomentCentered.interaction(2, 2);
114 
115  LI[11]=-3 * featureMomentCentered.interaction(4, 0) * momentCentered.get(2, 2) - 3 * momentCentered.get(4, 0) * featureMomentCentered.interaction(2, 2) - 2 * featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) - 2 * momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) + 6 * momentCentered.get(3, 1) * featureMomentCentered.interaction(3, 1) + 2 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) + 2 * momentCentered.get(3, 1) * featureMomentCentered.interaction(1, 3) - 3 * featureMomentCentered.interaction(2, 2) * momentCentered.get(0, 4) - 3 * momentCentered.get(2, 2) * featureMomentCentered.interaction(0, 4) + 6 * momentCentered.get(1, 3) * featureMomentCentered.interaction(1, 3);
116 
117 
118  LI[12]=6 * momentCentered.get(4, 0) * featureMomentCentered.interaction(4, 0) + 12 * featureMomentCentered.interaction(4, 0) * momentCentered.get(2, 2) + 12 * momentCentered.get(4, 0) * featureMomentCentered.interaction(2, 2) + 2 * featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) + 2 * momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) + 16 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) + 16 * momentCentered.get(3, 1) * featureMomentCentered.interaction(1, 3) + 12 * featureMomentCentered.interaction(2, 2) * momentCentered.get(0, 4) + 12 * momentCentered.get(2, 2) * featureMomentCentered.interaction(0, 4) + 6 * momentCentered.get(0, 4) * featureMomentCentered.interaction(0, 4);
119 
120 
121  LI[13]=2 * (momentCentered.get(5, 0) + 2 * momentCentered.get(3, 2) + momentCentered.get(1, 4)) * (featureMomentCentered.interaction(5, 0) + 2 * featureMomentCentered.interaction(3, 2) + featureMomentCentered.interaction(1, 4)) + 2 * (momentCentered.get(0, 5) + 2 * momentCentered.get(2, 3) + momentCentered.get(4, 1)) * (featureMomentCentered.interaction(0, 5) + 2 * featureMomentCentered.interaction(2, 3) + featureMomentCentered.interaction(4, 1));
122 
123  LI[14]=2 * (momentCentered.get(5, 0) - 2 * momentCentered.get(3, 2) - 3 * momentCentered.get(1, 4)) * (featureMomentCentered.interaction(5, 0) - 2 * featureMomentCentered.interaction(3, 2) - 3 * featureMomentCentered.interaction(1, 4)) + 2 * (momentCentered.get(0, 5) - 2 * momentCentered.get(2, 3) - 3 * momentCentered.get(4, 1)) * (featureMomentCentered.interaction(0, 5) - 2 * featureMomentCentered.interaction(2, 3) - 3 * featureMomentCentered.interaction(4, 1));
124 
125  LI[15]=2 * (momentCentered.get(5, 0) - 10 * momentCentered.get(3, 2) + 5 * momentCentered.get(1, 4)) * (featureMomentCentered.interaction(5, 0) - 10 * featureMomentCentered.interaction(3, 2) + 5 * featureMomentCentered.interaction(1, 4)) + 2 * (momentCentered.get(0, 5) - 10 * momentCentered.get(2, 3) + 5 * momentCentered.get(4, 1)) * (featureMomentCentered.interaction(0, 5) - 10 * featureMomentCentered.interaction(2, 3) + 5 * featureMomentCentered.interaction(4, 1));
126 
127  double s3 = momentCInvariant.getS(3);
128  double s2 = momentCInvariant.getS(2);
129  double c3 = momentCInvariant.getC(3);
130  double c2 = momentCInvariant.getC(2);
131  double I1 = momentCInvariant.getII(1);
132  double I2 = momentCInvariant.getII(2);
133  double I3 = momentCInvariant.getII(3);
134 
135  vpMatrix Lc2 = featureMomentCentered.interaction(0,3) - 3 * featureMomentCentered.interaction(2,1);
136  vpMatrix Ls2 = featureMomentCentered.interaction(3,0) - 3 * featureMomentCentered.interaction(1,2);
137  vpMatrix Lc3 = 2 * (momentCentered.get(2,0) - momentCentered.get(0,2)) * (featureMomentCentered.interaction(2,0) - featureMomentCentered.interaction(0,2)) - 8 * momentCentered.get(1,1) * featureMomentCentered.interaction(1,1);
138  vpMatrix Ls3 = 4 * featureMomentCentered.interaction(1,1) * (momentCentered.get(2,0) - momentCentered.get(0,2)) + 4 * momentCentered.get(1,1) * (featureMomentCentered.interaction(2,0) - featureMomentCentered.interaction(0,2));
139  vpMatrix LI1 = 2 * (momentCentered.get(2,0) - momentCentered.get(0,2)) * (featureMomentCentered.interaction(2,0) - featureMomentCentered.interaction(0,2)) + 8 * momentCentered.get(1,1) * featureMomentCentered.interaction(1,1);
140  vpMatrix LI2 = 2 * (momentCentered.get(0,3) - 3 * momentCentered.get(2,1)) * (featureMomentCentered.interaction(0,3) - 3 * featureMomentCentered.interaction(2,1)) + 2 * (momentCentered.get(3,0) - 3 * momentCentered.get(1,2)) * (featureMomentCentered.interaction(3,0) - 3 * featureMomentCentered.interaction(1,2));
141  vpMatrix LI3 = featureMomentCentered.interaction(2,0) + featureMomentCentered.interaction(0,2);
142 
143 
144  vpMatrix La(1,6);
145  double a;
146  if(momentObject.getType()==vpMomentObject::DISCRETE){
147  a = momentCentered.get(2,0)+momentCentered.get(0,2);
148  La = (featureMomentCentered.interaction(2,0)+featureMomentCentered.interaction(0,2));
149  }else{
150  a = momentObject.get(0,0);
151  La = featureMomentBasic.interaction(0,0);
152  }
153  interaction_matrices.resize(14);
154 
155 
156  interaction_matrices[0] = (1./(momentCInvariant.getI(2)*momentCInvariant.getI(2)))*(momentCInvariant.getI(2)*LI[1]-momentCInvariant.getI(1)*LI[2]);
157  interaction_matrices[1] = (1./(momentCInvariant.getI(4)*momentCInvariant.getI(4)))*(momentCInvariant.getI(4)*LI[3]-momentCInvariant.getI(3)*LI[4]);
158 
159  interaction_matrices[2] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[5]-momentCInvariant.getI(5)*LI[6]);
160 
161  interaction_matrices[3] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[7]-momentCInvariant.getI(7)*LI[6]);
162 
163  interaction_matrices[4] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[8]-momentCInvariant.getI(8)*LI[6]);
164 
165  interaction_matrices[5] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[9]-momentCInvariant.getI(9)*LI[6]);
166 
167 
168  interaction_matrices[6] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[11]-momentCInvariant.getI(11)*LI[10]);
169 
170  interaction_matrices[7] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[12]-momentCInvariant.getI(12)*LI[10]);
171 
172  interaction_matrices[8] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[13]-momentCInvariant.getI(13)*LI[15]);
173 
174  interaction_matrices[9] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[14]-momentCInvariant.getI(14)*LI[15]);
175 
176 
177  interaction_matrices[10] = (Lc2 * c3 + c2 * Lc3 + Ls2 * s3 + s2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (c2 * c3 + s2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (c2 * c3 + s2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (c2 * c3 + s2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
178 
179  interaction_matrices[11] = (Ls2 * c3 + s2 * Lc3 - Lc2 * s3 - c2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (s2 * c3 - c2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (s2 * c3 - c2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (s2 * c3 - c2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
180 
181  interaction_matrices[12] = (1/(I3*I3))*LI1-(2*I1/(I3*I3*I3))*LI3;
182  interaction_matrices[13] = (I2/(I3*I3*I3))*La+(a/(I3*I3*I3))*LI2-(3*a*I2/(I3*I3*I3*I3))*LI3;
183 }
184 
185 #else
186 #include <visp/vpFeatureMomentCInvariant.h>
187 #include <visp/vpMomentCentered.h>
188 #include <visp/vpMomentCInvariant.h>
189 #include <visp/vpFeatureMomentCentered.h>
190 #include <visp/vpFeatureMomentBasic.h>
191 
192 #include <visp/vpFeatureMomentDatabase.h>
193 
194 #include <vector>
195 #include <iostream>
196 #include <limits>
197 
198 
209 
210  std::vector<vpMatrix> LI(16);
211 
212  bool found_moment_centered;
213  bool found_moment_cinvariant;
214  bool found_FeatureMoment_centered;
215  bool found_featuremoment_basic;
216 
217  vpMomentObject& momentObject = moment->getObject();
218  vpMomentCentered& momentCentered = (static_cast<vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
219  vpMomentCInvariant& momentCInvariant = (static_cast<vpMomentCInvariant&>(moments.get("vpMomentCInvariant",found_moment_cinvariant)));
220  vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
221 
222  vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
223 
224  if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
225 
226 
227  if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
228  if(!found_moment_cinvariant) throw vpException(vpException::notInitialized,"vpMomentCInvariant not found");
229  if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
230 
231  vpMatrix zeros(1,6);
232  for(int i=0;i<6;i++) zeros[0][i]=0;
233 
234  double mu30 = momentCentered.get(3,0);
235  double mu30_2 = mu30*mu30;
236  double mu30_3 = mu30_2*mu30;
237 
238  double mu03 = momentCentered.get(0,3);
239  double mu03_2 = mu03*mu03;
240  double mu03_3 = mu03*mu03_2;
241 
242  double mu20 = momentCentered.get(2,0);
243  double mu02 = momentCentered.get(0,2);
244  double mu50 = momentCentered.get(5,0);
245  double mu32 = momentCentered.get(3,2);
246  double mu14 = momentCentered.get(1,4);
247  double mu05 = momentCentered.get(0,5);
248  double mu23 = momentCentered.get(2,3);
249  double mu41 = momentCentered.get(4,1);
250  double mu40 = momentCentered.get(4,0);
251  double mu04 = momentCentered.get(0,4);
252  double mu31 = momentCentered.get(3,1);
253  double mu13 = momentCentered.get(1,3);
254  double mu22 = momentCentered.get(2,2);
255  double mu21 = momentCentered.get(2,1);
256  double mu12 = momentCentered.get(1,2);
257  double mu11 = momentCentered.get(1,1);
258 
259 
260 
261  double mu12_2 = mu12*mu12;
262  double mu21_2 = mu21*mu21;
263  double mu21_3 = mu21*mu21_2;
264  double mu12_3 = mu12_2*mu12;
265 
266  vpMatrix Lmu20 = featureMomentCentered.interaction(2,0);
267  vpMatrix Lmu02 = featureMomentCentered.interaction(0,2);
268  vpMatrix Lmu11 = featureMomentCentered.interaction(1,1);
269  vpMatrix Lmu12 = featureMomentCentered.interaction(1,2);
270  vpMatrix Lmu03 = featureMomentCentered.interaction(0,3);
271  vpMatrix Lmu21 = featureMomentCentered.interaction(2,1);
272  vpMatrix Lmu30 = featureMomentCentered.interaction(3,0);
273  vpMatrix Lmu40 = featureMomentCentered.interaction(4,0);
274  vpMatrix Lmu04 = featureMomentCentered.interaction(0,4);
275  vpMatrix Lmu31 = featureMomentCentered.interaction(3,1);
276  vpMatrix Lmu13 = featureMomentCentered.interaction(1,3);
277  vpMatrix Lmu22 = featureMomentCentered.interaction(2,2);
278  vpMatrix Lmu05 = featureMomentCentered.interaction(0,5);
279  vpMatrix Lmu14 = featureMomentCentered.interaction(1,4);
280  vpMatrix Lmu23 = featureMomentCentered.interaction(2,3);
281  vpMatrix Lmu32 = featureMomentCentered.interaction(3,2);
282  vpMatrix Lmu41 = featureMomentCentered.interaction(4,1);
283  vpMatrix Lmu50 = featureMomentCentered.interaction(5,0);
284 
285 
286  LI[1]= -Lmu20*mu02-mu20*Lmu02+2*mu11*Lmu11;
287 
288  LI[2]= (-2*mu20+2*mu02)*Lmu02+8*mu11*Lmu11+(2*mu20-2*mu02)*Lmu20;
289 
290  LI[3]= (-6*mu21+2*mu03)*Lmu03+(-6*mu30+18*mu12)*Lmu12+(18*mu21-6*mu03)*Lmu21+(2*mu30-6*mu12)*Lmu30;
291 
292  LI[4]= (2*mu21+2*mu03)*Lmu03+(2*mu30+2*mu12)*Lmu12+(2*mu21+2*mu03)*Lmu21+(2*mu30+2*mu12)*Lmu30;
293 
294  LI[5]= (-2*mu30_2*mu03+6*mu30*mu21*mu12-4*mu21_3)*Lmu03+(6*mu30*mu21*mu03-12*mu30*mu12_2+6*mu21_2*mu12)*Lmu12+(6*mu30*mu12*mu03-12*mu21_2*mu03+6*mu21*mu12_2)*Lmu21+(-2*mu30*mu03_2-4*mu12_3+6*mu21*mu12*mu03)*Lmu30;
295 
296  LI[6]= (-6*mu30*mu21*mu12-6*mu21*mu12_2+6*mu21_2*mu03+2*mu21_3+4*mu30_2*mu03)*Lmu03+(-6*mu30*mu21_2-6*mu30*mu21*mu03+12*mu12_3+6*mu30_2*mu12-12*mu21*mu12*mu03+6*mu30*mu12_2)*Lmu12+(6*mu21*mu03_2+6*mu21_2*mu03-6*mu30*mu12*mu03+12*mu21_3-12*mu30*mu21*mu12-6*mu12_2*mu03)*Lmu21+(6*mu30*mu12_2+2*mu12_3+4*mu30*mu03_2-6*mu21_2*mu12-6*mu21*mu12*mu03)*Lmu30;
297 
298  LI[7] = (-6*mu21_2*mu12+3*mu30*mu03_2-mu30_3-3*mu30*mu21_2-6*mu21*mu12*mu03+3*mu30*mu12_2+2*mu12_3)*Lmu03+(-3*mu21*mu03_2+12*mu30*mu21*mu12+6*mu30*mu12*mu03+3*mu30_2*mu21+9*mu21*mu12_2-6*mu21_2*mu03-3*mu21_3+6*mu12_2*mu03)*Lmu12+(3*mu30_2*mu12-9*mu21_2*mu12-12*mu21*mu12*mu03-6*mu30*mu21*mu03-6*mu30*mu21_2+6*mu30*mu12_2+3*mu12_3-3*mu12*mu03_2)*Lmu21+(6*mu21*mu12_2+6*mu30*mu21*mu12-3*mu30_2*mu03+3*mu12_2*mu03-3*mu21_2*mu03-2*mu21_3+mu03_3)*Lmu30;
299 
300  LI[8]= (6*mu21_3-2*mu30*mu12*mu03+2*mu12_2*mu03+3*mu21*mu12_2-6*mu30*mu21*mu12-mu30_2*mu21-4*mu21_2*mu03-3*mu21*mu03_2)*Lmu03+(2*mu12*mu03_2-4*mu30_2*mu12+9*mu30*mu12_2-mu30*mu03_2-6*mu30*mu21*mu03+3*mu30*mu21_2+6*mu21*mu12*mu03-mu30_3)*Lmu12+(18*mu21_2*mu03+6*mu30*mu21*mu12-4*mu21*mu03_2-mu03_3-mu30_2*mu03-6*mu30*mu12*mu03+3*mu12_2*mu03+2*mu30_2*mu21)*Lmu21+(-6*mu21*mu12*mu03-4*mu30*mu12_2-2*mu30*mu21*mu03+2*mu30*mu21_2+3*mu12_3+3*mu21_2*mu12-3*mu30_2*mu12-mu12*mu03_2)*Lmu30;
301 
302  LI[9] = (2*(2*mu03+3*mu21))*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu03+6*mu30*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu12+6*mu03*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu21+(2*(2*mu30+3*mu12))*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu30;
303 
304  LI[10]= Lmu40*mu04+mu40*Lmu04-4*Lmu31*mu13-4*mu31*Lmu13+6*mu22*Lmu22;
305 
306  LI[11]= (-2*mu40-3*mu22)*Lmu04+(2*mu31+6*mu13)*Lmu13+(-3*mu04-3*mu40)*Lmu22+(2*mu13+6*mu31)*Lmu31+(-3*mu22-2*mu04)*Lmu40;
307 
308  LI[12]= (2*mu40+12*mu22+6*mu04)*Lmu04+16*mu31*Lmu13+(12*mu40+12*mu04)*Lmu22+16*Lmu31*mu13+(6*mu40+12*mu22+2*mu04)*Lmu40;
309 
310  LI[13]=(2*mu05+4*mu23+2*mu41)*Lmu05+(2*mu50+4*mu32+2*mu14)*Lmu14+(4*mu05+8*mu23+4*mu41)*Lmu23+(4*mu50+8*mu32+4*mu14)*Lmu32+(2*mu05+4*mu23+2*mu41)*Lmu41+(2*mu50+4*mu32+2*mu14)*Lmu50 ;
311 
312  LI[14]= (2*mu05-4*mu23-6*mu41)*Lmu05+(-6*mu50+12*mu32+18*mu14)*Lmu14+(-4*mu05+8*mu23+12*mu41)*Lmu23+(-4*mu50+8*mu32+12*mu14)*Lmu32+(-6*mu05+12*mu23+18*mu41)*Lmu41+(2*mu50-4*mu32-6*mu14)*Lmu50;
313 
314  LI[15]= (2*mu05-20*mu23+10*mu41)*Lmu05+(10*mu50-100*mu32+50*mu14)*Lmu14+(-20*mu05+200*mu23-100*mu41)*Lmu23+(-20*mu50+200*mu32-100*mu14)*Lmu32+(10*mu05-100*mu23+50*mu41)*Lmu41+(2*mu50-20*mu32+10*mu14)*Lmu50;
315 
316  double s3 = momentCInvariant.getS(3);
317  double s2 = momentCInvariant.getS(2);
318  double c3 = momentCInvariant.getC(3);
319  double c2 = momentCInvariant.getC(2);
320  double I1 = momentCInvariant.getII(1);
321  double I2 = momentCInvariant.getII(2);
322  double I3 = momentCInvariant.getII(3);
323 
324  vpMatrix Lmu20__Lmu02 = Lmu20 - Lmu02;
325  double mu20__mu02 = mu20 - mu02;
326  vpMatrix Lc2 = Lmu03 - 3 * Lmu21;
327  vpMatrix Ls2 = Lmu30 - 3 * Lmu12;
328  vpMatrix Lc3 = 2 * (mu20__mu02) * (Lmu20__Lmu02) - 8. * mu11 * Lmu11;
329  vpMatrix Ls3 = 4 * Lmu11 * (mu20__mu02) + 4 * mu11 * (Lmu20__Lmu02);
330  vpMatrix LI1 = 2 * (mu20__mu02) * (Lmu20__Lmu02) + 8 * mu11 * Lmu11;
331  vpMatrix LI2 = 2 * (mu03 - 3 * mu21) * (Lc2) + 2 * (mu30 - 3 * mu12) * (Ls2);
332  vpMatrix LI3 = Lmu20 + Lmu02;
333 
334 
335  vpMatrix La(1,6);
336  double a;
337  if(momentObject.getType()==vpMomentObject::DISCRETE){
338  a = momentCentered.get(2,0)+momentCentered.get(0,2);
339  La = (featureMomentCentered.interaction(2,0)+featureMomentCentered.interaction(0,2));
340  }else{
341  a = momentObject.get(0,0);
342  La = featureMomentBasic.interaction(0,0);
343  }
344  interaction_matrices.resize(14);
345 
346 
347  interaction_matrices[0] = (1./(momentCInvariant.getI(2)*momentCInvariant.getI(2)))*(momentCInvariant.getI(2)*LI[1]-momentCInvariant.getI(1)*LI[2]);
348  interaction_matrices[1] = (1./(momentCInvariant.getI(4)*momentCInvariant.getI(4)))*(momentCInvariant.getI(4)*LI[3]-momentCInvariant.getI(3)*LI[4]);
349 
350  interaction_matrices[2] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[5]-momentCInvariant.getI(5)*LI[6]);
351 
352  interaction_matrices[3] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[7]-momentCInvariant.getI(7)*LI[6]);
353 
354  interaction_matrices[4] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[8]-momentCInvariant.getI(8)*LI[6]);
355 
356  interaction_matrices[5] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[9]-momentCInvariant.getI(9)*LI[6]);
357 
358 
359  interaction_matrices[6] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[11]-momentCInvariant.getI(11)*LI[10]);
360 
361  interaction_matrices[7] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[12]-momentCInvariant.getI(12)*LI[10]);
362 
363  interaction_matrices[8] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[13]-momentCInvariant.getI(13)*LI[15]);
364 
365  interaction_matrices[9] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[14]-momentCInvariant.getI(14)*LI[15]);
366 
367 
368  interaction_matrices[10] = (Lc2 * c3 + c2 * Lc3 + Ls2 * s3 + s2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (c2 * c3 + s2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (c2 * c3 + s2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (c2 * c3 + s2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
369 
370  interaction_matrices[11] = (Ls2 * c3 + s2 * Lc3 - Lc2 * s3 - c2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (s2 * c3 - c2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (s2 * c3 - c2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (s2 * c3 - c2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
371 
372  interaction_matrices[12] = (1/(I3*I3))*LI1-(2*I1/(I3*I3*I3))*LI3;
373  interaction_matrices[13] = (I2/(I3*I3*I3))*La+(a/(I3*I3*I3))*LI2-(3*a*I2/(I3*I3*I3*I3))*LI3;
374 }
375 #endif
Functionality computation for basic moment feature. Computes the interaction matrix associated with v...
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
vpMomentObject & getObject() const
Definition: vpMoment.h:119
Functionality computation for centered moment feature. Computes the interaction matrix associated wit...
double getII(unsigned int i)
double get(unsigned int i, unsigned int j)
error that can be emited by ViSP classes.
Definition: vpException.h:75
Class for generic objects.
std::vector< vpMatrix > interaction_matrices
vpMoment & get(const char *type, bool &found)
std::vector< double > & get()
vpMomentDatabase & moments
double getS(unsigned int i)
This class defines the double-indexed centered moment descriptor .
vpObjectType getType() const
vpMatrix interaction(unsigned int select_one, unsigned int select_two)
vpFeatureMomentDatabase * featureMomentsDataBase
vpMatrix interaction(unsigned int select_one, unsigned int select_two)
double getC(unsigned int i)
double getI(unsigned int index)
This class defines several 2D (translation+rotation+scale) invariants for both symmetric and non-symm...
vpFeatureMoment & get(const char *type, bool &found)