ViSP  2.9.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 - 2014 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  const vpMomentObject& momentObject = moment->getObject();
72  const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
73  const vpMomentCInvariant& momentCInvariant = (static_cast<const vpMomentCInvariant&>(moments.get("vpMomentCInvariant",found_moment_cinvariant)));
74  vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
75 
76  vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->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 #include <cmath>
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  const vpMomentObject& momentObject = moment->getObject();
218  const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
219  const vpMomentCInvariant& momentCInvariant = (static_cast<const vpMomentCInvariant&>(moments.get("vpMomentCInvariant",found_moment_cinvariant)));
220 
221  vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
222 
223  vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
224 
225  if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
226  if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
227  if(!found_moment_cinvariant) throw vpException(vpException::notInitialized,"vpMomentCInvariant not found");
228  if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
229 
230  vpMatrix zeros(1,6);
231  for(int i=0;i<6;i++) zeros[0][i]=0;
232 
233  double mu30 = momentCentered.get(3,0);
234  double mu30_2 = mu30*mu30;
235  double mu30_3 = mu30_2*mu30;
236 
237  double mu03 = momentCentered.get(0,3);
238  double mu03_2 = mu03*mu03;
239  double mu03_3 = mu03*mu03_2;
240 
241  double mu20 = momentCentered.get(2,0);
242  double mu02 = momentCentered.get(0,2);
243  double mu50 = momentCentered.get(5,0);
244  double mu32 = momentCentered.get(3,2);
245  double mu14 = momentCentered.get(1,4);
246  double mu05 = momentCentered.get(0,5);
247  double mu23 = momentCentered.get(2,3);
248  double mu41 = momentCentered.get(4,1);
249  double mu40 = momentCentered.get(4,0);
250  double mu04 = momentCentered.get(0,4);
251  double mu31 = momentCentered.get(3,1);
252  double mu13 = momentCentered.get(1,3);
253  double mu22 = momentCentered.get(2,2);
254  double mu21 = momentCentered.get(2,1);
255  double mu12 = momentCentered.get(1,2);
256  double mu11 = momentCentered.get(1,1);
257 
258  double mu12_2 = mu12*mu12;
259  double mu21_2 = mu21*mu21;
260  double mu21_3 = mu21*mu21_2;
261  double mu12_3 = mu12_2*mu12;
262 
263  vpMatrix Lmu20 = featureMomentCentered.interaction(2,0);
264  vpMatrix Lmu02 = featureMomentCentered.interaction(0,2);
265  vpMatrix Lmu11 = featureMomentCentered.interaction(1,1);
266  vpMatrix Lmu12 = featureMomentCentered.interaction(1,2);
267  vpMatrix Lmu03 = featureMomentCentered.interaction(0,3);
268  vpMatrix Lmu21 = featureMomentCentered.interaction(2,1);
269  vpMatrix Lmu30 = featureMomentCentered.interaction(3,0);
270  vpMatrix Lmu40 = featureMomentCentered.interaction(4,0);
271  vpMatrix Lmu04 = featureMomentCentered.interaction(0,4);
272  vpMatrix Lmu31 = featureMomentCentered.interaction(3,1);
273  vpMatrix Lmu13 = featureMomentCentered.interaction(1,3);
274  vpMatrix Lmu22 = featureMomentCentered.interaction(2,2);
275  vpMatrix Lmu05 = featureMomentCentered.interaction(0,5);
276  vpMatrix Lmu14 = featureMomentCentered.interaction(1,4);
277  vpMatrix Lmu23 = featureMomentCentered.interaction(2,3);
278  vpMatrix Lmu32 = featureMomentCentered.interaction(3,2);
279  vpMatrix Lmu41 = featureMomentCentered.interaction(4,1);
280  vpMatrix Lmu50 = featureMomentCentered.interaction(5,0);
281 
282  LI[1]= -Lmu20*mu02-mu20*Lmu02+2*mu11*Lmu11;
283 
284  LI[2]= (-2*mu20+2*mu02)*Lmu02+8*mu11*Lmu11+(2*mu20-2*mu02)*Lmu20;
285 
286  LI[3]= (-6*mu21+2*mu03)*Lmu03+(-6*mu30+18*mu12)*Lmu12+(18*mu21-6*mu03)*Lmu21+(2*mu30-6*mu12)*Lmu30;
287 
288  LI[4]= (2*mu21+2*mu03)*Lmu03+(2*mu30+2*mu12)*Lmu12+(2*mu21+2*mu03)*Lmu21+(2*mu30+2*mu12)*Lmu30;
289 
290  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;
291 
292  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;
293 
294  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;
295 
296  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;
297 
298  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;
299 
300  LI[10]= Lmu40*mu04+mu40*Lmu04-4*Lmu31*mu13-4*mu31*Lmu13+6*mu22*Lmu22;
301 
302  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;
303 
304  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;
305 
306  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 ;
307 
308  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;
309 
310  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;
311 
312  double s3 = momentCInvariant.getS(3);
313  double s2 = momentCInvariant.getS(2);
314  double c3 = momentCInvariant.getC(3);
315  double c2 = momentCInvariant.getC(2);
316 
317  double I1 = momentCInvariant.getII(1);
318  double I2 = momentCInvariant.getII(2);
319  double I3 = momentCInvariant.getII(3);
320 
321  vpMatrix Lmu20__Lmu02 = Lmu20 - Lmu02;
322  double mu20__mu02 = mu20 - mu02;
323  vpMatrix Lc2 = Lmu03 - 3 * Lmu21;
324  vpMatrix Ls2 = Lmu30 - 3 * Lmu12;
325  vpMatrix Lc3 = 2 * (mu20__mu02) * (Lmu20__Lmu02) - 8. * mu11 * Lmu11;
326  vpMatrix Ls3 = 4 * Lmu11 * (mu20__mu02) + 4 * mu11 * (Lmu20__Lmu02);
327  vpMatrix LI1 = 2 * (mu20__mu02) * (Lmu20__Lmu02) + 8 * mu11 * Lmu11;
328  vpMatrix LI2 = 2 * (mu03 - 3 * mu21) * (Lc2) + 2 * (mu30 - 3 * mu12) * (Ls2);
329  vpMatrix LI3 = Lmu20 + Lmu02;
330 
331  vpMatrix La(1,6);
332  double a;
333  if(momentObject.getType()==vpMomentObject::DISCRETE){
334  a = momentCentered.get(2,0)+momentCentered.get(0,2);
335  La = (featureMomentCentered.interaction(2,0)+featureMomentCentered.interaction(0,2));
336  }else{
337  a = momentObject.get(0,0);
338  La = featureMomentBasic.interaction(0,0);
339  }
340 
341  interaction_matrices.resize(14);
342 
343  interaction_matrices[0] = (1./(momentCInvariant.getI(2)*momentCInvariant.getI(2)))*(momentCInvariant.getI(2)*LI[1]-momentCInvariant.getI(1)*LI[2]);
344  interaction_matrices[1] = (1./(momentCInvariant.getI(4)*momentCInvariant.getI(4)))*(momentCInvariant.getI(4)*LI[3]-momentCInvariant.getI(3)*LI[4]);
345 
346  interaction_matrices[2] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[5]-momentCInvariant.getI(5)*LI[6]);
347 
348  interaction_matrices[3] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[7]-momentCInvariant.getI(7)*LI[6]);
349 
350  interaction_matrices[4] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[8]-momentCInvariant.getI(8)*LI[6]);
351 
352  interaction_matrices[5] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[9]-momentCInvariant.getI(9)*LI[6]);
353 
354  interaction_matrices[6] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[11]-momentCInvariant.getI(11)*LI[10]);
355 
356  interaction_matrices[7] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[12]-momentCInvariant.getI(12)*LI[10]);
357 
358  interaction_matrices[8] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[13]-momentCInvariant.getI(13)*LI[15]);
359 
360  interaction_matrices[9] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[14]-momentCInvariant.getI(14)*LI[15]);
361 
362  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;
363 
364  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;
365 
366  interaction_matrices[12] = (1/(I3*I3))*LI1-(2*I1/(I3*I3*I3))*LI3;
367 
368  interaction_matrices[13] = (I2/(I3*I3*I3))*La+(a/(I3*I3*I3))*LI2-(3*a*I2/(I3*I3*I3*I3))*LI3;
369 }
370 #endif
Functionality computation for basic moment feature. Computes the interaction matrix associated with v...
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
double getS(unsigned int i) const
const vpMoment * moment
Functionality computation for centered moment feature. Computes the interaction matrix associated wit...
error that can be emited by ViSP classes.
Definition: vpException.h:76
Class for generic objects.
const std::vector< double > & get() const
std::vector< vpMatrix > interaction_matrices
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
vpMatrix interaction(unsigned int select_one, unsigned int select_two) const
double getI(unsigned int index) const
double getC(unsigned int i) const
const vpMoment & get(const char *type, bool &found) const
vpMomentDatabase & moments
This class defines the double-indexed centered moment descriptor .
double get(unsigned int i, unsigned int j) const
double getII(unsigned int i) const
vpObjectType getType() const
vpFeatureMomentDatabase * featureMomentsDataBase
const vpMomentObject & getObject() const
Definition: vpMoment.h:123
vpFeatureMoment & get(const char *type, bool &found)