GCC Code Coverage Report

Directory: ./
File: src/feature/feature-visual-point.cpp
Date: 2025-05-13 12:28:21
Exec Total Coverage
Lines: 18 84 21.4%
Branches: 20 128 15.6%
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 4 L(),
32
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 xySIN(NULL, "sotFeatureVisualPoint(" + name + ")::input(vector)::xy"),
33
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 ZSIN(NULL, "sotFeatureVisualPoint(" + name + ")::input(double)::Z"),
34
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 articularJacobianSIN(
35
3/6
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 12 NULL, "sotFeatureVisualPoint(" + name + ")::input(matrix)::Jq") {
36
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 ZSIN = 1.;
37
38
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 jacobianSOUT.addDependency(xySIN);
39
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 jacobianSOUT.addDependency(ZSIN);
40
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 jacobianSOUT.addDependency(articularJacobianSIN);
41
42
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 errorSOUT.addDependency(xySIN);
43
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 errorSOUT.addDependency(ZSIN);
44
45
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 signalRegistration(xySIN << ZSIN << articularJacobianSIN);
46 4 }
47
48 2 void FeatureVisualPoint::addDependenciesFromReference(void) {
49
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 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 */
187