GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Copyright 2010, |
||
3 |
* François Bleibel, |
||
4 |
* Olivier Stasse, |
||
5 |
* |
||
6 |
* CNRS/AIST |
||
7 |
* |
||
8 |
*/ |
||
9 |
|||
10 |
/* --------------------------------------------------------------------- */ |
||
11 |
/* --- INCLUDE --------------------------------------------------------- */ |
||
12 |
/* --------------------------------------------------------------------- */ |
||
13 |
|||
14 |
/* --- SOT --- */ |
||
15 |
#include <sot/core/debug.hh> |
||
16 |
#include <sot/core/exception-feature.hh> |
||
17 |
#include <sot/core/factory.hh> |
||
18 |
#include <sot/core/feature-visual-point.hh> |
||
19 |
using namespace std; |
||
20 |
using namespace dynamicgraph::sot; |
||
21 |
using namespace dynamicgraph; |
||
22 |
|||
23 |
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureVisualPoint, "FeatureVisualPoint"); |
||
24 |
|||
25 |
/* --------------------------------------------------------------------- */ |
||
26 |
/* --- CLASS ----------------------------------------------------------- */ |
||
27 |
/* --------------------------------------------------------------------- */ |
||
28 |
|||
29 |
4 |
FeatureVisualPoint::FeatureVisualPoint(const string &pointName) |
|
30 |
: FeatureAbstract(pointName), |
||
31 |
L(), |
||
32 |
✓✗ | 8 |
xySIN(NULL, "sotFeatureVisualPoint(" + name + ")::input(vector)::xy"), |
33 |
✓✗ | 8 |
ZSIN(NULL, "sotFeatureVisualPoint(" + name + ")::input(double)::Z"), |
34 |
articularJacobianSIN( |
||
35 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
20 |
NULL, "sotFeatureVisualPoint(" + name + ")::input(matrix)::Jq") { |
36 |
✓✗ | 4 |
ZSIN = 1.; |
37 |
|||
38 |
✓✗ | 4 |
jacobianSOUT.addDependency(xySIN); |
39 |
✓✗ | 4 |
jacobianSOUT.addDependency(ZSIN); |
40 |
✓✗ | 4 |
jacobianSOUT.addDependency(articularJacobianSIN); |
41 |
|||
42 |
✓✗ | 4 |
errorSOUT.addDependency(xySIN); |
43 |
✓✗ | 4 |
errorSOUT.addDependency(ZSIN); |
44 |
|||
45 |
✓✗✓✗ ✓✗ |
4 |
signalRegistration(xySIN << ZSIN << articularJacobianSIN); |
46 |
4 |
} |
|
47 |
|||
48 |
2 |
void FeatureVisualPoint::addDependenciesFromReference(void) { |
|
49 |
✗✓ | 2 |
assert(isReferenceSet()); |
50 |
2 |
errorSOUT.addDependency(getReference()->xySIN); |
|
51 |
2 |
} |
|
52 |
|||
53 |
void FeatureVisualPoint::removeDependenciesFromReference(void) { |
||
54 |
assert(isReferenceSet()); |
||
55 |
errorSOUT.removeDependency(getReference()->xySIN); |
||
56 |
} |
||
57 |
|||
58 |
/* --------------------------------------------------------------------- */ |
||
59 |
/* --------------------------------------------------------------------- */ |
||
60 |
/* --------------------------------------------------------------------- */ |
||
61 |
|||
62 |
unsigned int &FeatureVisualPoint::getDimension(unsigned int &dim, int time) { |
||
63 |
sotDEBUG(25) << "# In {" << endl; |
||
64 |
|||
65 |
const Flags &fl = selectionSIN.access(time); |
||
66 |
|||
67 |
dim = 0; |
||
68 |
if (fl(0)) dim++; |
||
69 |
if (fl(1)) dim++; |
||
70 |
|||
71 |
sotDEBUG(25) << "# Out }" << endl; |
||
72 |
return dim; |
||
73 |
} |
||
74 |
|||
75 |
/** Compute the interaction matrix from a subset of |
||
76 |
* the possible features. |
||
77 |
*/ |
||
78 |
Matrix &FeatureVisualPoint::computeJacobian(Matrix &J, int time) { |
||
79 |
sotDEBUG(15) << "# In {" << endl; |
||
80 |
|||
81 |
sotDEBUG(15) << "Get selection flags." << endl; |
||
82 |
const Flags &fl = selectionSIN(time); |
||
83 |
|||
84 |
const int dim = dimensionSOUT(time); |
||
85 |
L.resize(dim, 6); |
||
86 |
unsigned int cursorL = 0; |
||
87 |
|||
88 |
sotDEBUG(5) << std::endl; |
||
89 |
|||
90 |
const double &Z = ZSIN(time); |
||
91 |
sotDEBUG(5) << xySIN(time) << std::endl; |
||
92 |
const double &x = xySIN(time)(0); |
||
93 |
const double &y = xySIN(time)(1); |
||
94 |
|||
95 |
if (Z < 0) { |
||
96 |
throw(ExceptionFeature(ExceptionFeature::BAD_INIT, |
||
97 |
"VisualPoint is behind the camera", " (Z=%.1f).", |
||
98 |
Z)); |
||
99 |
} |
||
100 |
|||
101 |
if (fabs(Z) < 1e-6) { |
||
102 |
throw(ExceptionFeature(ExceptionFeature::BAD_INIT, |
||
103 |
"VisualPoint Z coordinates is null", " (Z=%.3f)", |
||
104 |
Z)); |
||
105 |
} |
||
106 |
|||
107 |
if (fl(0)) { |
||
108 |
L(cursorL, 0) = -1 / Z; |
||
109 |
L(cursorL, 1) = 0; |
||
110 |
L(cursorL, 2) = x / Z; |
||
111 |
L(cursorL, 3) = x * y; |
||
112 |
L(cursorL, 4) = -(1 + x * x); |
||
113 |
L(cursorL, 5) = y; |
||
114 |
|||
115 |
cursorL++; |
||
116 |
} |
||
117 |
|||
118 |
if (fl(1)) { |
||
119 |
L(cursorL, 0) = 0; |
||
120 |
L(cursorL, 1) = -1 / Z; |
||
121 |
L(cursorL, 2) = y / Z; |
||
122 |
L(cursorL, 3) = 1 + y * y; |
||
123 |
L(cursorL, 4) = -x * y; |
||
124 |
L(cursorL, 5) = -x; |
||
125 |
|||
126 |
cursorL++; |
||
127 |
} |
||
128 |
sotDEBUG(15) << "L:" << endl << L << endl; |
||
129 |
sotDEBUG(15) << "Jq:" << endl << articularJacobianSIN(time) << endl; |
||
130 |
|||
131 |
J = L * articularJacobianSIN(time); |
||
132 |
|||
133 |
sotDEBUG(15) << "# Out }" << endl; |
||
134 |
return J; |
||
135 |
} |
||
136 |
|||
137 |
/** Compute the error between two visual features from a subset |
||
138 |
* a the possible features. |
||
139 |
*/ |
||
140 |
Vector &FeatureVisualPoint::computeError(Vector &error, int time) { |
||
141 |
const Flags &fl = selectionSIN(time); |
||
142 |
sotDEBUGIN(15); |
||
143 |
error.resize(dimensionSOUT(time)); |
||
144 |
unsigned int cursorL = 0; |
||
145 |
|||
146 |
if (!isReferenceSet()) { |
||
147 |
throw(ExceptionFeature(ExceptionFeature::BAD_INIT, |
||
148 |
"S* is not of adequate type.")); |
||
149 |
} |
||
150 |
|||
151 |
if (fl(0)) { |
||
152 |
error(cursorL++) = xySIN(time)(0) - getReference()->xySIN(time)(0); |
||
153 |
} |
||
154 |
if (fl(1)) { |
||
155 |
error(cursorL++) = xySIN(time)(1) - getReference()->xySIN(time)(1); |
||
156 |
} |
||
157 |
|||
158 |
sotDEBUGOUT(15); |
||
159 |
return error; |
||
160 |
} |
||
161 |
|||
162 |
void FeatureVisualPoint::display(std::ostream &os) const { |
||
163 |
os << "VisualPoint <" << name << ">:"; |
||
164 |
|||
165 |
try { |
||
166 |
const Vector &xy = xySIN.accessCopy(); |
||
167 |
const Flags &fl = selectionSIN.accessCopy(); |
||
168 |
if (fl(0)) os << " x=" << xy(0); |
||
169 |
if (fl(1)) os << " y=" << xy(1); |
||
170 |
} catch (ExceptionAbstract e) { |
||
171 |
os << " XY or select not set."; |
||
172 |
} |
||
173 |
|||
174 |
try { |
||
175 |
const double &z = ZSIN.accessCopy(); |
||
176 |
os << " Z=" << z << " "; |
||
177 |
} catch (ExceptionAbstract e) { |
||
178 |
os << " Z not set."; |
||
179 |
} |
||
180 |
} |
||
181 |
|||
182 |
/* |
||
183 |
* Local variables: |
||
184 |
* c-basic-offset: 2 |
||
185 |
* End: |
||
186 |
*/ |
Generated by: GCOVR (Version 4.2) |