GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/feature/feature-line-distance.cpp Lines: 0 172 0.0 %
Date: 2023-03-13 12:09:37 Branches: 0 226 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 <sot/core/debug.hh>
16
#include <sot/core/exception-feature.hh>
17
#include <sot/core/feature-line-distance.hh>
18
#include <sot/core/matrix-geometry.hh>
19
20
using namespace std;
21
22
using namespace dynamicgraph::sot;
23
using namespace dynamicgraph;
24
25
#include <sot/core/factory.hh>
26
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureLineDistance, "FeatureLineDistance");
27
28
/* --------------------------------------------------------------------- */
29
/* --- CLASS ----------------------------------------------------------- */
30
/* --------------------------------------------------------------------- */
31
32
FeatureLineDistance::FeatureLineDistance(const string &pointName)
33
    : FeatureAbstract(pointName),
34
      positionSIN(NULL, "sotFeatureLineDistance(" + name +
35
                            ")::input(matrixHomo)::position"),
36
      articularJacobianSIN(
37
          NULL, "sotFeatureLineDistance(" + name + ")::input(matrix)::Jq"),
38
      positionRefSIN(NULL, "sotFeatureLineDistance(" + name +
39
                               ")::input(vector)::positionRef"),
40
      vectorSIN(NULL,
41
                "sotFeatureVector3(" + name + ")::input(vector3)::vector"),
42
      lineSOUT(boost::bind(&FeatureLineDistance::computeLineCoordinates, this,
43
                           _1, _2),
44
               positionSIN << positionRefSIN,
45
               "sotFeatureAbstract(" + name + ")::output(vector)::line") {
46
  jacobianSOUT.addDependency(positionSIN);
47
  jacobianSOUT.addDependency(articularJacobianSIN);
48
49
  errorSOUT.addDependency(positionSIN);
50
51
  signalRegistration(positionSIN << articularJacobianSIN << positionRefSIN
52
                                 << lineSOUT << vectorSIN);
53
}
54
55
/* --------------------------------------------------------------------- */
56
/* --------------------------------------------------------------------- */
57
/* --------------------------------------------------------------------- */
58
59
unsigned int &FeatureLineDistance::getDimension(unsigned int &dim,
60
                                                int /*time*/) {
61
  sotDEBUG(25) << "# In {" << endl;
62
63
  return dim = 1;
64
}
65
66
/* --------------------------------------------------------------------- */
67
Vector &FeatureLineDistance::computeLineCoordinates(Vector &cood, int time) {
68
  sotDEBUGIN(15);
69
70
  cood.resize(6);
71
72
  /* Line coordinates */
73
  const MatrixHomogeneous &pos = positionSIN(time);
74
  const Vector &vect = vectorSIN(time);
75
  MatrixRotation R;
76
  R = pos.linear();
77
  Vector v(3);
78
  v = R * vect;
79
80
  cood(0) = pos(0, 3);
81
  cood(1) = pos(1, 3);
82
  cood(2) = pos(2, 3);
83
  cood(3) = v(0);
84
  cood(4) = v(1);
85
  cood(5) = v(2);
86
87
  sotDEBUGOUT(15);
88
  return cood;
89
}
90
91
/* --------------------------------------------------------------------- */
92
/** Compute the interaction matrix from a subset of
93
 * the possible features.
94
 */
95
Matrix &FeatureLineDistance::computeJacobian(Matrix &J, int time) {
96
  sotDEBUG(15) << "# In {" << endl;
97
98
  /* --- Compute the jacobian of the line coordinates --- */
99
  Matrix Jline;
100
  {
101
    const Matrix &Jq = articularJacobianSIN(time);
102
103
    const Vector &vect = vectorSIN(time);
104
    const MatrixHomogeneous &M = positionSIN(time);
105
    MatrixRotation R;
106
    R = M.linear();  // wRh
107
108
    Matrix Skew(3, 3);
109
    Skew(0, 0) = 0;
110
    Skew(0, 1) = -vect(2);
111
    Skew(0, 2) = vect(1);
112
    Skew(1, 0) = vect(2);
113
    Skew(1, 1) = 0;
114
    Skew(1, 2) = -vect(0);
115
    Skew(2, 0) = -vect(1);
116
    Skew(2, 1) = vect(0);
117
    Skew(2, 2) = 0;
118
119
    Matrix RSk(3, 3);
120
    RSk = R * Skew;
121
122
    Jline.resize(6, Jq.cols());
123
    for (unsigned int i = 0; i < 3; ++i)
124
      for (int j = 0; j < Jq.cols(); ++j) {
125
        Jline(i, j) = 0;
126
        Jline(i + 3, j) = 0;
127
        for (unsigned int k = 0; k < 3; ++k) {
128
          Jline(i, j) += R(i, k) * Jq(k, j);
129
          Jline(i + 3, j) += -RSk(i, k) * Jq(k + 3, j);
130
        }
131
      }
132
  }
133
134
  /* --- Compute the jacobian wrt the line coordinates --- */
135
  const Vector &line = lineSOUT(time);
136
  const double &x0 = line(0);
137
  const double &y0 = line(1);
138
  const double &z0 = line(2);
139
  const double &a0 = line(3);
140
  const double &b0 = line(4);
141
  const double &c0 = line(5);
142
143
  const Vector &posRef = positionRefSIN(time);
144
  const double &x1 = posRef(0);
145
  const double &y1 = posRef(1);
146
  const double &z1 = posRef(2);
147
  const double &a1 = posRef(3);
148
  const double &b1 = posRef(4);
149
  const double &c1 = posRef(5);
150
151
  /* Differential */
152
  const double a1_3 = a1 * a1 * a1;
153
  const double b1_3 = b1 * b1 * b1;
154
  const double c1_3 = c1 * c1 * c1;
155
156
  double K = c0 * c0 * a1 * a1 - 2 * c0 * a1 * a0 * c1 - 2 * c0 * b1 * b0 * c1 +
157
             c0 * c0 * b1 * b1 - 2 * b0 * a1 * a0 * b1 + b0 * b0 * a1 * a1 +
158
             b0 * b0 * c1 * c1 + a0 * a0 * b1 * b1 + a0 * a0 * c1 * c1;
159
160
  const double diffx0 = -b0 * c1 + c0 * b1;
161
  const double diffy0 = a0 * c1 - c0 * a1;
162
  const double diffz0 = -a0 * b1 + b0 * a1;
163
164
  const double diffa0 =
165
      2 * b0 * c1 * x0 * a0 * b1 * b1 + 2 * c0 * b1 * b1 * x0 * b0 * a1 +
166
      2 * c0 * c0 * b1 * x0 * c1 * a1 + 2 * c1 * c1 * y0 * c0 * a1 * a0 -
167
      2 * b0 * c1 * c1 * x0 * c0 * a1 - 2 * b0 * b0 * c1 * x0 * b1 * a1 -
168
      2 * c1 * c1 * y0 * c0 * b1 * b0 + 2 * b0 * b0 * c1 * x1 * b1 * a1 +
169
      2 * b0 * c1 * c1 * x1 * c0 * a1 - 2 * b0 * c1 * x1 * a0 * b1 * b1 -
170
      c1 * y0 * c0 * c0 * a1 * a1 + c1 * y0 * c0 * c0 * b1 * b1 +
171
      c1 * y0 * b0 * b0 * a1 * a1 - c1 * y0 * a0 * a0 * b1 * b1 +
172
      c1 * y1 * c0 * c0 * a1 * a1 - c1 * y1 * c0 * c0 * b1 * b1 -
173
      c1 * y1 * b0 * b0 * a1 * a1 + c1 * y1 * a0 * a0 * b1 * b1 -
174
      b1 * z0 * c0 * c0 * a1 * a1 + b1 * z0 * b0 * b0 * a1 * a1 -
175
      b1 * z0 * b0 * b0 * c1 * c1 + b1 * z0 * a0 * a0 * c1 * c1 +
176
      b1 * z1 * c0 * c0 * a1 * a1 - b1 * z1 * b0 * b0 * a1 * a1 +
177
      b1 * z1 * b0 * b0 * c1 * c1 - b1 * z1 * a0 * a0 * c1 * c1 +
178
      2 * b0 * c1_3 * x0 * a0 - 2 * b0 * c1_3 * x1 * a0 -
179
      2 * c0 * b1_3 * x0 * a0 + 2 * c0 * b1_3 * x1 * a0 + c1_3 * y0 * b0 * b0 -
180
      c1_3 * y0 * a0 * a0 - c1_3 * y1 * b0 * b0 + c1_3 * y1 * a0 * a0 -
181
      b1_3 * z0 * c0 * c0 + b1_3 * z0 * a0 * a0 + b1_3 * z1 * c0 * c0 -
182
      b1_3 * z1 * a0 * a0 - 2 * c1 * c1 * y1 * c0 * a1 * a0 +
183
      2 * c1 * c1 * y1 * c0 * b1 * b0 + 2 * b1 * b1 * z0 * c0 * b0 * c1 -
184
      2 * b1 * b1 * z0 * b0 * a1 * a0 - 2 * b1 * b1 * z1 * c0 * b0 * c1 +
185
      2 * b1 * b1 * z1 * b0 * a1 * a0 - 2 * c0 * b1 * x0 * a0 * c1 * c1 -
186
      2 * c0 * b1 * b1 * x1 * b0 * a1 - 2 * c0 * c0 * b1 * x1 * c1 * a1 +
187
      2 * c0 * b1 * x1 * a0 * c1 * c1 + 2 * c0 * a1 * y0 * a0 * b1 * b1 -
188
      2 * c0 * a1 * a1 * y0 * b1 * b0 - 2 * c0 * a1 * y1 * a0 * b1 * b1 +
189
      2 * c0 * a1 * a1 * y1 * b1 * b0 + 2 * b0 * a1 * a1 * z0 * c1 * c0 -
190
      2 * b0 * a1 * z0 * a0 * c1 * c1 - 2 * b0 * a1 * a1 * z1 * c1 * c0 +
191
      2 * b0 * a1 * z1 * a0 * c1 * c1;
192
  const double diffb0 =
193
      -2 * c1 * c1 * x0 * c0 * b1 * b0 + 2 * c1 * c1 * x0 * c0 * a1 * a0 -
194
      c1 * x0 * c0 * c0 * a1 * a1 + c1 * x0 * c0 * c0 * b1 * b1 +
195
      c1 * x0 * b0 * b0 * a1 * a1 - c1 * x0 * a0 * a0 * b1 * b1 +
196
      c1 * x1 * c0 * c0 * a1 * a1 - c1 * x1 * c0 * c0 * b1 * b1 -
197
      c1 * x1 * b0 * b0 * a1 * a1 + c1 * x1 * a0 * a0 * b1 * b1 +
198
      a1 * z0 * c0 * c0 * b1 * b1 - a1 * z0 * b0 * b0 * c1 * c1 -
199
      a1 * z0 * a0 * a0 * b1 * b1 + a1 * z0 * a0 * a0 * c1 * c1 -
200
      a1 * z1 * c0 * c0 * b1 * b1 + a1 * z1 * b0 * b0 * c1 * c1 +
201
      a1 * z1 * a0 * a0 * b1 * b1 - a1 * z1 * a0 * a0 * c1 * c1 -
202
      2 * a0 * c1_3 * y0 * b0 + 2 * a0 * c1_3 * y1 * b0 + c1_3 * x0 * b0 * b0 -
203
      c1_3 * x0 * a0 * a0 - c1_3 * x1 * b0 * b0 + c1_3 * x1 * a0 * a0 +
204
      a1_3 * z0 * c0 * c0 - 2 * c1 * c1 * x1 * c0 * a1 * a0 +
205
      2 * c1 * c1 * x1 * c0 * b1 * b0 - 2 * a1 * a1 * z0 * c0 * a0 * c1 +
206
      2 * a1 * a1 * z0 * b0 * a0 * b1 - a1_3 * z0 * b0 * b0 -
207
      a1_3 * z1 * c0 * c0 + a1_3 * z1 * b0 * b0 +
208
      2 * a1 * a1 * z1 * c0 * a0 * c1 - 2 * a1 * a1 * z1 * b0 * a0 * b1 +
209
      2 * c0 * b1 * b1 * x0 * a1 * a0 - 2 * c0 * b1 * x0 * b0 * a1 * a1 -
210
      2 * c0 * b1 * b1 * x1 * a1 * a0 + 2 * c0 * b1 * x1 * b0 * a1 * a1 +
211
      2 * a0 * a0 * c1 * y0 * a1 * b1 - 2 * a0 * c1 * y0 * b0 * a1 * a1 +
212
      2 * a0 * c1 * c1 * y0 * c0 * b1 - 2 * a0 * a0 * c1 * y1 * a1 * b1 +
213
      2 * a0 * c1 * y1 * b0 * a1 * a1 - 2 * a0 * c1 * c1 * y1 * c0 * b1 -
214
      2 * c0 * a1 * a1 * y0 * a0 * b1 + 2 * c0 * a1_3 * y0 * b0 -
215
      2 * c0 * a1_3 * y1 * b0 + 2 * c0 * a1 * y0 * b0 * c1 * c1 -
216
      2 * c0 * c0 * a1 * y0 * c1 * b1 + 2 * c0 * a1 * a1 * y1 * a0 * b1 -
217
      2 * c0 * a1 * y1 * b0 * c1 * c1 + 2 * c0 * c0 * a1 * y1 * c1 * b1 +
218
      2 * a0 * b1 * z0 * b0 * c1 * c1 - 2 * a0 * b1 * b1 * z0 * c1 * c0 -
219
      2 * a0 * b1 * z1 * b0 * c1 * c1 + 2 * a0 * b1 * b1 * z1 * c1 * c0;
220
221
  Matrix diffh(1, 6);
222
  diffh(0, 0) = diffx0 / K;
223
  diffh(0, 1) = diffy0 / K;
224
  diffh(0, 2) = diffz0 / K;
225
  K *= K;
226
  diffh(0, 3) = diffa0 / K;
227
  diffh(0, 4) = diffb0 / K;
228
  diffh(0, 5) = 0;
229
230
  /* --- Multiply Jline=dline/dq with diffh=de/dline --- */
231
  J.resize(1, J.cols());
232
  J = diffh * Jline;
233
  // J=Jline;
234
235
  sotDEBUG(15) << "# Out }" << endl;
236
  return J;
237
}
238
239
/** Compute the error between two visual features from a subset
240
 *a the possible features.
241
 */
242
Vector &FeatureLineDistance::computeError(Vector &error, int time) {
243
  sotDEBUGIN(15);
244
245
  /* Line coordinates */
246
  const Vector &line = lineSOUT(time);
247
  const double &x0 = line(0);
248
  const double &y0 = line(1);
249
  const double &z0 = line(2);
250
  const double &a0 = line(3);
251
  const double &b0 = line(4);
252
  const double &c0 = line(5);
253
254
  const Vector &posRef = positionRefSIN(time);
255
  const double &x1 = posRef(0);
256
  const double &y1 = posRef(1);
257
  const double &z1 = posRef(2);
258
  const double &a1 = posRef(3);
259
  const double &b1 = posRef(4);
260
  const double &c1 = posRef(5);
261
262
  error.resize(1);
263
  double K = c0 * c0 * a1 * a1 - 2 * c0 * a1 * a0 * c1 - 2 * c0 * b1 * b0 * c1 +
264
             c0 * c0 * b1 * b1 - 2 * b0 * a1 * a0 * b1 + b0 * b0 * a1 * a1 +
265
             b0 * b0 * c1 * c1 + a0 * a0 * b1 * b1 + a0 * a0 * c1 * c1;
266
  error(0) = (-b0 * c1 * x0 + b0 * c1 * x1 + c0 * b1 * x0 - c0 * b1 * x1 +
267
              a0 * c1 * y0 - a0 * c1 * y1 - c0 * a1 * y0 + c0 * a1 * y1 -
268
              a0 * b1 * z0 + a0 * b1 * z1 + b0 * a1 * z0 - b0 * a1 * z1) /
269
             K;
270
271
  /* --- DEBUG --- */
272
  //   error.resize(6);
273
  //   error=line-posRef;
274
275
  sotDEBUGOUT(15);
276
  return error;
277
}
278
279
void FeatureLineDistance::display(std::ostream &os) const {
280
  os << "LineDistance <" << name << ">";
281
}