GCC Code Coverage Report

Directory: ./
File: src/feature/feature-point6d-relative.cpp
Date: 2025-05-13 12:28:21
Exec Total Coverage
Lines: 0 148 0.0%
Branches: 0 396 0.0%
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 <dynamic-graph/all-commands.h>
16 #include <dynamic-graph/pool.h>
17
18 #include <sot/core/debug.hh>
19 #include <sot/core/exception-feature.hh>
20 #include <sot/core/feature-point6d-relative.hh>
21 #include <sot/core/macros.hh>
22 #include <sot/core/matrix-geometry.hh>
23
24 using namespace std;
25 using namespace dynamicgraph::sot;
26 using namespace dynamicgraph;
27 namespace dg = dynamicgraph;
28
29 #include <sot/core/factory.hh>
30 SOT_CORE_DISABLE_WARNING_PUSH
31 SOT_CORE_DISABLE_WARNING_DEPRECATED
32 ✗ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoint6dRelative,
33 "FeaturePoint6dRelative");
34 SOT_CORE_DISABLE_WARNING_POP
35
36 /* --------------------------------------------------------------------- */
37 /* --- CLASS ----------------------------------------------------------- */
38 /* --------------------------------------------------------------------- */
39
40 ✗ FeaturePoint6dRelative::FeaturePoint6dRelative(const string &pointName)
41 : FeaturePoint6d(pointName),
42 ✗ positionReferenceSIN(NULL, "sotFeaturePoint6dRelative(" + name +
43 ")::input(matrixHomo)::positionRef"),
44 ✗ articularJacobianReferenceSIN(NULL, "sotFeaturePoint6dRelative(" + name +
45 ")::input(matrix)::JqRef"),
46 ✗ dotpositionSIN(NULL, "sotFeaturePoint6dRelative(" + name +
47 ")::input(matrixHomo)::dotposition"),
48 ✗ dotpositionReferenceSIN(NULL,
49 ✗ "sotFeaturePoint6dRelative(" + name +
50 ✗ ")::input(matrixHomo)::dotpositionRef") {
51 ✗ jacobianSOUT.addDependency(positionReferenceSIN);
52 ✗ jacobianSOUT.addDependency(articularJacobianReferenceSIN);
53
54 ✗ errorSOUT.addDependency(positionReferenceSIN);
55
56 ✗ errordotSOUT.addDependency(dotpositionReferenceSIN);
57
58 ✗ signalRegistration(positionReferenceSIN << articularJacobianReferenceSIN);
59
60 ✗ signalRegistration(dotpositionSIN << dotpositionReferenceSIN << errordotSOUT);
61
62 ✗ errordotSOUT.setFunction(
63 boost::bind(&FeaturePoint6dRelative::computeErrordot, this, _1, _2));
64 ✗ initCommands();
65 }
66
67 /* --------------------------------------------------------------------- */
68 /* --------------------------------------------------------------------- */
69 /* --------------------------------------------------------------------- */
70
71 /** Compute the interaction matrix from a subset of
72 * the possible features.
73 */
74 ✗ Matrix &FeaturePoint6dRelative::computeJacobian(Matrix &Jres, int time) {
75 sotDEBUG(15) << "# In {" << endl;
76
77 ✗ const Matrix &Jq = articularJacobianSIN(time);
78 ✗ const Matrix &JqRef = articularJacobianReferenceSIN(time);
79 ✗ const MatrixHomogeneous &wMp = positionSIN(time);
80 ✗ const MatrixHomogeneous &wMpref = positionReferenceSIN(time);
81
82 ✗ const Matrix::Index cJ = Jq.cols();
83 ✗ Matrix J(6, cJ);
84 {
85 ✗ MatrixHomogeneous pMw;
86 ✗ pMw = wMp.inverse(Eigen::Affine);
87 ✗ MatrixHomogeneous pMpref;
88 ✗ pMpref = pMw * wMpref;
89
90 ✗ MatrixTwist pVpref;
91 ✗ buildFrom(pMpref, pVpref);
92 ✗ J = pVpref * JqRef;
93 ✗ J -= Jq;
94 }
95
96 ✗ const Flags &fl = selectionSIN(time);
97 ✗ const int dim = dimensionSOUT(time);
98 sotDEBUG(15) << "Dimension=" << dim << std::endl;
99 ✗ Jres.resize(dim, cJ);
100
101 ✗ unsigned int rJ = 0;
102 ✗ for (unsigned int r = 0; r < 6; ++r)
103 ✗ if (fl(r)) {
104 ✗ for (unsigned int c = 0; c < cJ; ++c) Jres(rJ, c) = J(r, c);
105 ✗ rJ++;
106 }
107
108 sotDEBUG(15) << "# Out }" << endl;
109 ✗ return Jres;
110 }
111
112 /** Compute the error between two visual features from a subset
113 * a the possible features.
114 */
115 ✗ Vector &FeaturePoint6dRelative::computeError(Vector &error, int time) {
116 sotDEBUGIN(15);
117
118 // /* TODO */
119 // error.resize(6); error.fill(.0);
120
121 ✗ const MatrixHomogeneous &wMp = positionSIN(time);
122 ✗ const MatrixHomogeneous &wMpref = positionReferenceSIN(time);
123
124 ✗ MatrixHomogeneous pMw;
125 ✗ pMw = wMp.inverse(Eigen::Affine);
126 ✗ MatrixHomogeneous pMpref;
127 ✗ pMpref = pMw * wMpref;
128
129 ✗ MatrixHomogeneous Merr;
130 try {
131 ✗ if (isReferenceSet()) {
132 // TODO: Deal with the case of FeaturePoint6dRelative reference without
133 // dcast
134 FeaturePoint6dRelative *sdes6d =
135 ✗ dynamic_cast<FeaturePoint6dRelative *>(getReference());
136 ✗ if (NULL != sdes6d) {
137 ✗ const MatrixHomogeneous &wMp_des = sdes6d->positionSIN(time);
138 const MatrixHomogeneous &wMpref_des =
139 ✗ sdes6d->positionReferenceSIN(time);
140 ✗ MatrixHomogeneous pMw_des;
141 ✗ pMw_des = wMp_des.inverse(Eigen::Affine);
142 ✗ MatrixHomogeneous pMpref_des;
143 ✗ pMpref_des = pMw_des * wMpref_des;
144 ✗ MatrixHomogeneous Minv;
145 ✗ Minv = pMpref_des.inverse(Eigen::Affine);
146 ✗ Merr = pMpref * Minv;
147 } else {
148 ✗ const MatrixHomogeneous &Mref = getReference()->positionSIN(time);
149 ✗ MatrixHomogeneous Minv;
150 ✗ Minv = Mref.inverse(Eigen::Affine);
151 ✗ Merr = pMpref * Minv;
152 }
153 } else {
154 ✗ Merr = pMpref;
155 }
156 ✗ } catch (...) {
157 ✗ Merr = pMpref;
158 }
159
160 ✗ MatrixRotation Rerr;
161 ✗ Rerr = Merr.linear();
162 ✗ VectorUTheta rerr(Rerr);
163
164 ✗ const Flags &fl = selectionSIN(time);
165 ✗ error.resize(dimensionSOUT(time));
166 ✗ unsigned int cursor = 0;
167 ✗ for (unsigned int i = 0; i < 3; ++i) {
168 ✗ if (fl(i)) error(cursor++) = Merr(i, 3);
169 }
170 ✗ for (unsigned int i = 0; i < 3; ++i) {
171 ✗ if (fl(i + 3)) error(cursor++) = rerr.angle() * rerr.axis()(i);
172 }
173
174 sotDEBUGOUT(15);
175 ✗ return error;
176 }
177
178 /** Compute the error between two visual features from a subset
179 * a the possible features.
180 *
181 * This is computed by the desired feature.
182 */
183 ✗ Vector &FeaturePoint6dRelative::computeErrorDot(Vector &errordot, int time) {
184 sotDEBUGIN(15);
185
186 // /* TODO */
187 // error.resize(6); error.fill(.0);
188 ✗ const MatrixHomogeneous &wMp = positionSIN(time);
189 ✗ const MatrixHomogeneous &wMpref = positionReferenceSIN(time);
190 ✗ const MatrixHomogeneous &wdMp = dotpositionSIN(time);
191 ✗ const MatrixHomogeneous &wdMpref = dotpositionReferenceSIN(time);
192
193 sotDEBUG(15) << "wdMp :" << wdMp << endl;
194 sotDEBUG(15) << "wdMpref :" << wdMpref << endl;
195
196 ✗ MatrixRotation dRerr;
197 ✗ Vector dtrerr;
198
199 try {
200 ✗ MatrixRotation wRp;
201 ✗ wRp = wMp.linear();
202 ✗ MatrixRotation wRpref;
203 ✗ wRpref = wMpref.linear();
204 ✗ MatrixRotation wdRp;
205 ✗ wdRp = wdMp.linear();
206 ✗ MatrixRotation wdRpref;
207 ✗ wdRpref = wdMpref.linear();
208
209 ✗ Vector trp(3);
210 ✗ trp = wMp.translation();
211 ✗ Vector trpref(3);
212 ✗ trpref = wMpref.translation();
213 ✗ Vector trdp(3);
214 ✗ trdp = wdMp.translation();
215 ✗ Vector trdpref(3);
216 ✗ trdpref = wdMpref.translation();
217
218 sotDEBUG(15) << "Everything is extracted" << endl;
219 ✗ MatrixRotation wdRpt, wRpt, op1, op2;
220 ✗ wdRpt = wdRp.transpose();
221 ✗ op1 = wdRpt * wRpref;
222 ✗ wRpt = wRp.transpose();
223 ✗ op2 = wRpt * wdRpref;
224 ✗ dRerr = op1 + op2;
225
226 sotDEBUG(15) << "dRerr" << dRerr << endl;
227 ✗ Vector trtmp1(3), vop1(3), vop2(3);
228 ✗ trtmp1 = trpref - trp;
229 ✗ vop1 = wdRpt * trtmp1;
230 ✗ trtmp1 = trdpref - trdp;
231 ✗ vop2 = wRpt * trtmp1;
232 ✗ dtrerr = vop1 - vop2;
233
234 sotDEBUG(15) << "dtrerr" << dtrerr << endl;
235
236 ✗ } catch (...) {
237 sotDEBUG(15) << "You've got a problem with errordot." << std::endl;
238 }
239
240 ✗ VectorUTheta rerr(dRerr);
241
242 ✗ const Flags &fl = selectionSIN(time);
243 ✗ errordot.resize(dimensionSOUT(time));
244 ✗ unsigned int cursor = 0;
245 ✗ for (unsigned int i = 0; i < 3; ++i) {
246 ✗ if (fl(i)) errordot(cursor++) = dtrerr(i);
247 }
248 ✗ for (unsigned int i = 0; i < 3; ++i) {
249 ✗ if (fl(i + 3)) errordot(cursor++) = rerr.angle() * rerr.axis()(i);
250 }
251
252 sotDEBUGOUT(15);
253 ✗ return errordot;
254 }
255
256 static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
257 ✗ void FeaturePoint6dRelative::display(std::ostream &os) const {
258 ✗ os << "Point6dRelative <" << name << ">: (";
259
260 try {
261 ✗ const Flags &fl = selectionSIN.accessCopy();
262 ✗ bool first = true;
263 ✗ for (int i = 0; i < 6; ++i)
264 ✗ if (fl(i)) {
265 ✗ if (first) {
266 ✗ first = false;
267 } else {
268 ✗ os << ",";
269 }
270 ✗ os << featureNames[i];
271 }
272 ✗ os << ") ";
273 ✗ } catch (ExceptionAbstract e) {
274 ✗ os << " selectSIN not set.";
275 }
276 }
277
278 ✗ void FeaturePoint6dRelative::initCommands(void) {
279 using namespace command;
280 ✗ addCommand("initSdes", makeCommandVoid1(
281 *this, &FeaturePoint6dRelative::initSdes,
282 ✗ docCommandVoid1("Initialize the desired feature.",
283 "string (desired feature name)")));
284 }
285
286 /* Initialise the reference value: set up the sdes signal of the current
287 * feature, and freezes the position and position-reference of the desired
288 * feature.
289 */
290 ✗ void FeaturePoint6dRelative::initSdes(const std::string &nameSdes) {
291 ✗ FeaturePoint6dRelative &sdes = dynamic_cast<FeaturePoint6dRelative &>(
292 ✗ dg::PoolStorage::getInstance()->getEntity(nameSdes));
293
294 ✗ setReference(&sdes);
295
296 ✗ const int timeCurr = positionSIN.getTime() + 1;
297 ✗ positionSIN.recompute(timeCurr);
298 ✗ positionReferenceSIN.recompute(timeCurr);
299
300 ✗ sdes.positionSIN.setConstant(positionSIN.accessCopy());
301 ✗ sdes.positionReferenceSIN.setConstant(positionReferenceSIN.accessCopy());
302 }
303