GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/feature/feature-vector3.cpp Lines: 0 52 0.0 %
Date: 2023-03-13 12:09:37 Branches: 0 134 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
//#define VP_DEBUG
16
//#define VP_DEBUG_MODE 45
17
#include <sot/core/debug.hh>
18
#include <sot/core/exception-feature.hh>
19
#include <sot/core/factory.hh>
20
#include <sot/core/feature-vector3.hh>
21
#include <sot/core/matrix-geometry.hh>
22
23
using namespace dynamicgraph::sot;
24
using namespace std;
25
using namespace dynamicgraph;
26
27
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureVector3, "FeatureVector3");
28
29
/* --------------------------------------------------------------------- */
30
/* --- CLASS ----------------------------------------------------------- */
31
/* --------------------------------------------------------------------- */
32
33
FeatureVector3::FeatureVector3(const string &pointName)
34
    : FeatureAbstract(pointName),
35
      vectorSIN(NULL,
36
                "sotFeatureVector3(" + name + ")::input(vector3)::vector"),
37
      positionSIN(
38
          NULL, "sotFeaturePoint6d(" + name + ")::input(matrixHomo)::position"),
39
      articularJacobianSIN(
40
          NULL, "sotFeatureVector3(" + name + ")::input(matrix)::Jq"),
41
      positionRefSIN(
42
          NULL, "sotFeatureVector3(" + name + ")::input(vector)::positionRef") {
43
  jacobianSOUT.addDependency(positionSIN);
44
  jacobianSOUT.addDependency(articularJacobianSIN);
45
46
  errorSOUT.addDependency(vectorSIN);
47
  errorSOUT.addDependency(positionSIN);
48
  errorSOUT.addDependency(positionRefSIN);
49
50
  signalRegistration(vectorSIN << positionSIN << articularJacobianSIN
51
                               << positionRefSIN);
52
}
53
54
/* --------------------------------------------------------------------- */
55
/* --------------------------------------------------------------------- */
56
/* --------------------------------------------------------------------- */
57
58
unsigned int &FeatureVector3::getDimension(unsigned int &dim, int /*time*/) {
59
  sotDEBUG(25) << "# In {" << endl;
60
61
  return dim = 3;
62
}
63
64
/* --------------------------------------------------------------------- */
65
/** Compute the interaction matrix from a subset of
66
 * the possible features.
67
 */
68
Matrix &FeatureVector3::computeJacobian(Matrix &J, int time) {
69
  sotDEBUG(15) << "# In {" << endl;
70
71
  const Matrix &Jq = articularJacobianSIN(time);
72
  const Vector &vect = vectorSIN(time);
73
  const MatrixHomogeneous &M = positionSIN(time);
74
  MatrixRotation R;
75
  R = M.linear();
76
77
  Matrix Skew(3, 3);
78
  Skew(0, 0) = 0;
79
  Skew(0, 1) = -vect(2);
80
  Skew(0, 2) = vect(1);
81
  Skew(1, 0) = vect(2);
82
  Skew(1, 1) = 0;
83
  Skew(1, 2) = -vect(0);
84
  Skew(2, 0) = -vect(1);
85
  Skew(2, 1) = vect(0);
86
  Skew(2, 2) = 0;
87
88
  Matrix RSk(3, 3);
89
  RSk = R * Skew;
90
91
  J.resize(3, Jq.cols());
92
  for (unsigned int i = 0; i < 3; ++i)
93
    for (int j = 0; j < Jq.cols(); ++j) {
94
      J(i, j) = 0;
95
      for (unsigned int k = 0; k < 3; ++k) {
96
        J(i, j) -= RSk(i, k) * Jq(k + 3, j);
97
      }
98
    }
99
100
  sotDEBUG(15) << "# Out }" << endl;
101
  return J;
102
}
103
104
/** Compute the error between two visual features from a subset
105
 *a the possible features.
106
 */
107
Vector &FeatureVector3::computeError(Vector &Mvect3, int time) {
108
  sotDEBUGIN(15);
109
110
  const MatrixHomogeneous &M = positionSIN(time);
111
  const Vector &vect = vectorSIN(time);
112
  const Vector &vectdes = positionRefSIN(time);
113
114
  sotDEBUG(15) << "M = " << M << std::endl;
115
  sotDEBUG(15) << "v = " << vect << std::endl;
116
  sotDEBUG(15) << "vd = " << vectdes << std::endl;
117
118
  MatrixRotation R;
119
  R = M.linear();
120
  Mvect3.resize(3);
121
  Mvect3 = R * vect;
122
  Mvect3 -= vectdes;
123
124
  sotDEBUGOUT(15);
125
  return Mvect3;
126
}
127
128
void FeatureVector3::display(std::ostream &os) const {
129
  os << "Vector3 <" << name << ">";
130
}