GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/feature/feature-visual-point.cpp Lines: 16 82 19.5 %
Date: 2023-03-13 12:09:37 Branches: 20 124 16.1 %

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
 */