Directory: | ./ |
---|---|
File: | src/feature/feature-generic.cpp |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 35 | 65 | 53.8% |
Branches: | 36 | 118 | 30.5% |
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-generic.hh> | ||
19 | |||
20 | using namespace std; | ||
21 | using namespace dynamicgraph::sot; | ||
22 | using namespace dynamicgraph; | ||
23 | ✗ | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureGeneric, "FeatureGeneric"); | |
24 | |||
25 | /* --------------------------------------------------------------------- */ | ||
26 | /* --- CLASS ----------------------------------------------------------- */ | ||
27 | /* --------------------------------------------------------------------- */ | ||
28 | |||
29 | 2 | FeatureGeneric::FeatureGeneric(const string &pointName) | |
30 | : FeatureAbstract(pointName), | ||
31 | 2 | dimensionDefault(0), | |
32 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | errorSIN(NULL, "sotFeatureGeneric(" + name + ")::input(vector)::errorIN"), |
33 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | jacobianSIN(NULL, |
34 |
3/6✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
|
6 | "sotFeatureGeneric(" + name + ")::input(matrix)::jacobianIN") |
35 | |||
36 | { | ||
37 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | jacobianSOUT.addDependency(jacobianSIN); |
38 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | errorSOUT.addDependency(errorSIN); |
39 | |||
40 |
4/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
2 | signalRegistration(errorSIN << jacobianSIN << errordotSIN << errordotSOUT); |
41 | 2 | } | |
42 | |||
43 | /* --------------------------------------------------------------------- */ | ||
44 | /* --------------------------------------------------------------------- */ | ||
45 | /* --------------------------------------------------------------------- */ | ||
46 | |||
47 | 1 | void FeatureGeneric::addDependenciesFromReference(void) { | |
48 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | assert(SP::isReferenceSet()); |
49 | 1 | errorSOUT.addDependency(getReference()->errorSIN); | |
50 | 1 | errordotSOUT.addDependency(getReference()->errordotSIN); | |
51 | 1 | } | |
52 | |||
53 | ✗ | void FeatureGeneric::removeDependenciesFromReference(void) { | |
54 | ✗ | assert(SP::isReferenceSet()); | |
55 | ✗ | errorSOUT.removeDependency(getReference()->errorSIN); | |
56 | ✗ | errordotSOUT.removeDependency(getReference()->errordotSIN); | |
57 | } | ||
58 | |||
59 | /* --------------------------------------------------------------------- */ | ||
60 | /* --------------------------------------------------------------------- */ | ||
61 | /* --------------------------------------------------------------------- */ | ||
62 | |||
63 | 1 | unsigned int &FeatureGeneric::getDimension(unsigned int &dim, int time) { | |
64 | sotDEBUG(25) << "# In {" << endl; | ||
65 | |||
66 | 1 | const Flags &fl = selectionSIN.access(time); | |
67 | |||
68 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (dimensionDefault == 0) dimensionDefault = errorSIN.access(time).size(); |
69 | |||
70 | 1 | dim = 0; | |
71 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
|
7 | for (unsigned int i = 0; i < dimensionDefault; ++i) |
72 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
6 | if (fl(i)) dim++; |
73 | |||
74 | sotDEBUG(25) << "# Out }" << endl; | ||
75 | 1 | return dim; | |
76 | } | ||
77 | |||
78 | 10 | Vector &FeatureGeneric::computeError(Vector &res, int time) { | |
79 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | const Vector &err = errorSIN.access(time); |
80 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | const Flags &fl = selectionSIN.access(time); |
81 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | const int &dim = dimensionSOUT(time); |
82 | |||
83 | 10 | unsigned int curr = 0; | |
84 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | res.resize(dim); |
85 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 10 times.
|
10 | if (err.size() < dim) { |
86 | ✗ | SOT_THROW ExceptionFeature( | |
87 | ✗ | ExceptionFeature::UNCOMPATIBLE_SIZE, | |
88 | "Error: dimension uncompatible with des->errorIN size." | ||
89 | " (while considering feature <%s>).", | ||
90 | ✗ | getName().c_str()); | |
91 | } | ||
92 | |||
93 | sotDEBUG(15) << "Err = " << err; | ||
94 | sotDEBUG(25) << "Dim = " << dim << endl; | ||
95 | |||
96 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
|
10 | if (isReferenceSet()) { |
97 |
1/2✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
|
10 | const Vector &errDes = getReference()->errorSIN(time); |
98 | sotDEBUG(15) << "Err* = " << errDes; | ||
99 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 10 times.
|
10 | if (errDes.size() < dim) { |
100 | ✗ | SOT_THROW ExceptionFeature( | |
101 | ✗ | ExceptionFeature::UNCOMPATIBLE_SIZE, | |
102 | "Error: dimension uncompatible with des->errorIN size." | ||
103 | " (while considering feature <%s>).", | ||
104 | ✗ | getName().c_str()); | |
105 | } | ||
106 | |||
107 |
2/2✓ Branch 1 taken 60 times.
✓ Branch 2 taken 10 times.
|
70 | for (int i = 0; i < err.size(); ++i) |
108 |
2/4✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
|
60 | if (fl(i)) |
109 |
5/10✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 60 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 60 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 60 times.
✗ Branch 13 not taken.
|
60 | if (fl(i)) res(curr++) = err(i) - errDes(i); |
110 | } else { | ||
111 | ✗ | for (int i = 0; i < err.size(); ++i) | |
112 | ✗ | if (fl(i)) res(curr++) = err(i); | |
113 | } | ||
114 | |||
115 | 10 | return res; | |
116 | } | ||
117 | |||
118 | ✗ | Matrix &FeatureGeneric::computeJacobian(Matrix &res, int time) { | |
119 | sotDEBUGIN(15); | ||
120 | |||
121 | ✗ | const Matrix &Jac = jacobianSIN.access(time); | |
122 | ✗ | const Flags &fl = selectionSIN.access(time); | |
123 | ✗ | const unsigned int &dim = dimensionSOUT(time); | |
124 | |||
125 | ✗ | unsigned int curr = 0; | |
126 | ✗ | res.resize(dim, Jac.cols()); | |
127 | |||
128 | ✗ | for (unsigned int i = 0; curr < dim; ++i) | |
129 | ✗ | if (fl(i)) { | |
130 | ✗ | for (int j = 0; j < Jac.cols(); ++j) res(curr, j) = Jac(i, j); | |
131 | ✗ | curr++; | |
132 | } | ||
133 | |||
134 | sotDEBUGOUT(15); | ||
135 | ✗ | return res; | |
136 | } | ||
137 | |||
138 | /* --------------------------------------------------------------------- */ | ||
139 | /* --------------------------------------------------------------------- */ | ||
140 | /* --------------------------------------------------------------------- */ | ||
141 | |||
142 | ✗ | void FeatureGeneric::display(std::ostream &os) const { | |
143 | ✗ | os << "Generic <" << name << ">: " << std::endl; | |
144 | |||
145 | try { | ||
146 | ✗ | os << " error= " << errorSIN.accessCopy() << endl | |
147 | ✗ | << " J = " << jacobianSIN.accessCopy() << endl; | |
148 | ✗ | } catch (ExceptionSignal &e) { | |
149 | ✗ | os << e.what(); | |
150 | } | ||
151 | } | ||
152 |