kbm.cpp
Go to the documentation of this file.
1 /* *********************
2  * @file kbm.cpp
3  *
4  * @author Stefan Ulbrich
5  * @date 2013-2014
6  *
7  * @brief This file contains the implementation the Kinematic Bezier Maps minus the methods related to subdivision.
8  * *********************/
9 #include "kbm.h"
10 #include <string>
11 #include <sstream>
12 #include <cmath>
13 #include <assert.h>
14 #include <fstream>
15 #include <vector>
16 #include <iomanip>
17 #include <algorithm> // std::sort
18 
21 
22 namespace memoryx::KBM::Models
23 {
24  std::ostream& operator<<(std::ostream& os, const KBM::ErrorValuesType& et)
25  {
26  os << "Mean: " << et.Mean << ", STD:" << et.STD << ", Median: " << et.Median << ", IQR: " << et.IQR << ", Max: " << et.Max << std::endl;
27  return os;
28  }
29 
30  KBM::KBM(const KBM& other)
31  {
32  this->nControlPoints = other.nControlPoints;
33  this->nDoF = other.nDoF;
34  this->dim = other.dim;
35  this->controlNet = other.controlNet;
36  this->spreadAngles = other.spreadAngles;
37  this->index = other.index;
38  this->center = other.center;
39  this->partialDerivatives = other.partialDerivatives;
40  }
41 
42  KBM::KBM(int nDoF, int dim, Real spreadAngle)
43  {
44  this->nDoF = nDoF;
45  this->dim = dim;
46  //this->spreadAngle = spreadAngle;
47  //this->spreadAngles = std::vector<Real>(nDoF,spreadAngle);
48  this->spreadAngles = Vector::Constant(nDoF, spreadAngle);
49  this->nControlPoints = pow(3.0, this->nDoF);
50  // ARMARX_DEBUG_S << "Creating KBM with" << this->nDoF << " degrees of freedom and " << this->dim << " output dimensions." << std::endl;
51  this->controlNet = Matrix::Zero(this->dim, this->nControlPoints);
52  this->createIndices();
53  this->center = Vector::Zero(this->nDoF);
54  this->partialDerivatives = std::vector<Matrix>(this->nDoF, Matrix::Zero(this->dim, this->nControlPoints));
55  }
56 
57  KBM::ErrorValuesType KBM::getErrors(const Matrix& proprioception, const Matrix& shape)
58  {
59  int nSamples = shape.cols();
60  // ARMARX_DEBUG_S << this->spreadAngle << std::endl;
61  ErrorValuesType error_values;
62 
63  Matrix errors = this->controlNet * this->createSLE(proprioception) - shape;
64  //ARMARX_DEBUG_S << errors;
65  std::vector<Real> summed_squared_errors(nSamples, 0.0f);
66 
67  error_values.Mean = 0; //mean squared error
68 
69  for (int iSamples = 0; iSamples < nSamples; iSamples++)
70  {
71  Real sum = 0.0;
72 
73  for (int iDim = 0; iDim < this->dim; iDim++)
74  {
75  sum += pow(errors(iDim, iSamples), 2);
76  //ARMARX_DEBUG_S << pow(errors(iDim,iSamples),2) << std::endl;
77  }
78 
79  summed_squared_errors[iSamples] = sqrt(sum);
80  error_values.Mean += sqrt(sum);
81  }
82 
83  error_values.Mean /= nSamples;
84  std::sort(summed_squared_errors.begin(), summed_squared_errors.end());
85  //for (int i =0; i<summed_squared_errors.size(); i++) ARMARX_DEBUG_S << summed_squared_errors[i] << std::endl;
86  error_values.Median = summed_squared_errors[nSamples / 2];
87  error_values.IQR = summed_squared_errors[(nSamples * 3) / 4 ] - summed_squared_errors[nSamples / 4];
88  error_values.STD = 0;
89  error_values.Max = *std::max_element(summed_squared_errors.begin(), summed_squared_errors.end());
90  //ARMARX_DEBUG_S << error_values << std::endl;
91  return error_values;
92  }
93 
94 
95 
96  void KBM::createIndices()
97  {
98 
99  this->index.resize(nControlPoints, nDoF);
100 
101  for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
102  {
103  int decimal = iControlPoints;//-1;
104 
105  //ARMARX_DEBUG_S << decimal << std::endl;
106  for (int iDoF = 0; iDoF < nDoF; iDoF++)
107  {
108  //ARMARX_DEBUG_S << iDoF << std::endl;
109  this->index(iControlPoints, iDoF) = decimal % 3;
110  //ARMARX_DEBUG_S << this->index(iControlPoints,iDoF) << " ";
111  decimal /= 3;
112  }
113 
114  //ARMARX_DEBUG_S<<std::endl;
115  }
116 
117  //ARMARX_DEBUG_S << this->index << std::endl;
118 
119  }
120 
121 
122  Matrix KBM::createSLE(const Matrix& proprioception, int dim, Real a1, Real a2, bool project) const
123  {
124  assert(proprioception.rows() == nDoF);
125  int nSamples = proprioception.cols();
126  Matrix SLE(nControlPoints, nSamples);
127 
128 
129  Real bincoeff[] = {1, 2, 1};
130 
131  for (int iSamples = 0; iSamples < nSamples; iSamples++)
132  {
133  Real weight = 0;
134 
135  for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
136  {
137  SLE(iControlPoints, iSamples) = 1.0;
138 
139  // ARMARX_DEBUG_S << SLE;
140  for (int iDoF = 0; iDoF < this->nDoF; iDoF++)
141  {
142  int idx = this->index(iControlPoints, iDoF);
143 
144  if (dim != iDoF)
145  {
146  Real t = std::tan((proprioception(iDoF, iSamples) - center(iDoF)) / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
147  Real gamma;
148 
149  if (idx == 1)
150  {
151  gamma = std::cos(this->spreadAngles[iDoF]);
152  }
153  else
154  {
155  gamma = 1;
156  }
157 
158  //ARMARX_DEBUG_S << idx << " "<< t << " " << this->spreadAngle << " " << gamma << std::endl;
159  SLE(iControlPoints, iSamples) *= gamma * bincoeff[idx] * pow(t, idx) * pow(1.0 - t, 2 - idx); // Bernstein polynomial
160  }
161  else //Blossom
162  {
163  Real t1 = std::tan(a1 / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
164  Real t2 = std::tan(a2 / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
165 
166  switch (idx)
167  {
168  case 0:
169  SLE(iControlPoints, iSamples) *= (1 - t1) * (1 - t2);
170  break;
171 
172  case 1:
173  SLE(iControlPoints, iSamples) *= ((1 - t1) * t2 + (1 - t2) * t1) * std::cos(this->spreadAngles[iDoF]);
174  break;
175 
176  case 2:
177  SLE(iControlPoints, iSamples) *= t1 * t2;
178  break;
179  }
180  }
181  }
182 
183  //ARMARX_DEBUG_S << SLE;
184  weight += SLE(iControlPoints, iSamples);
185  }
186 
187  //ARMARX_DEBUG_S << "Weight: " << weight << std::endl;
188  if (project)
189  for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
190  {
191  SLE(iControlPoints, iSamples) /= weight;
192  }
193  }
194 
195  //ARMARX_DEBUG_S << SLE << std::endl;
196  return SLE;
197  }
198 
199 
200  void KBM::batch(const Matrix& proprioception, const Matrix& shape, Optimization method, Real threshold)
201  {
202  assert(proprioception.cols() == shape.cols());
203  assert(proprioception.rows() == this->nDoF);
204  assert(shape.rows() == this->dim);
205  Matrix SLE = this->createSLE(proprioception);
206 
207  switch (method)
208  {
209  case KBM::STANDARD:
210  this->controlNet = SLE.transpose().householderQr().solve(shape.transpose()).transpose();
211  break;
212 
213  case KBM::PLS:
214  this->controlNet = PLS::solve(SLE, shape, threshold);
215  break;
216  }
217 
218  // ARMARX_DEBUG_S << "Control Net:\n" << this->controlNet << std::endl;
219  }
220 
221  void KBM::online(const Matrix& proprioception, const Matrix& shape, Real learnRate)
222  {
223  assert(proprioception.cols() == shape.cols());
224  assert(proprioception.rows() == this->nDoF);
225  // ARMARX_IMPORTANT_S << VAROUT(shape.rows()) << VAROUT(this->dim);
226  ARMARX_CHECK_EXPRESSION(shape.rows() == this->dim) << armarx::ValueToString(shape.rows()) + " " + armarx::ValueToString(this->dim);
227 
228  //ARMARX_DEBUG_S << "proprioception: " << proprioception<<std::endl;
229  //ARMARX_DEBUG_S << "shape: " << shape<<std::endl;
230  int nSamples = proprioception.cols();
231  Matrix SLE = createSLE(proprioception);
232 
233  //ARMARX_DEBUG_S << this->dim << std::endl;
234  for (int iSamples = 0; iSamples < nSamples; iSamples++)
235  {
236  for (int iDim = 0; iDim < this->dim ; iDim++)
237  {
238  Vector temp = SLE.col(iSamples);
239  Real delta = shape(iDim, iSamples) - this->controlNet.row(iDim).dot(temp);
240  Matrix update(this->nControlPoints, 1);
241  update = delta * SLE.col(iSamples) * (1.0 / SLE.col(iSamples).dot(SLE.col(iSamples)));
242  //ARMARX_DEBUG_S << iSamples << std::endl << iDim << std::endl << delta << std::endl << update << std::endl;
243  this->controlNet.row(iDim) += learnRate * update.transpose();
244  }
245  }
246  }
247 
248  bool KBM::restore(std::string fileName)
249  {
250  std::ifstream file(fileName.c_str());
251 
252  if (!file.is_open())
253  {
254  // ARMARX_DEBUG_S << "Could not open file: " << fileName << std::endl;
255  return false;
256  }
257 
258  std::vector<double> values;
259 
260  // ARMARX_DEBUG_S << "Reading " << fileName << std::endl;
261  int lastCols = -1; //Compare the number of columns in each row.
262  int rows = 0;
263  std::string line;
264 
265  while (getline(file, line))
266  {
267  int cols = 0;
268  std::stringstream linestream(line);
269  std::string value;
270 
271  while (getline(linestream, value, ','))
272  {
273  values.push_back(strtod(value.c_str(), NULL));
274  cols++;
275  }
276 
277  rows++;
278 
279 
280  // check if number of columns changes.
281  if (lastCols >= 0)
282  {
283  assert(lastCols == cols);
284  }
285 
286  lastCols = cols;
287  }
288 
289  //ARMARX_DEBUG_S << values.size() << std::endl;
290  assert(this->dim == rows && "Incompatible output dimensions");
291  assert(this->nControlPoints == lastCols && "Incompatible input dimensions");
292 
293  for (int i = 0; i < this->dim; i++)
294  for (int j = 0; j < this->nControlPoints; j++)
295  {
296  this->controlNet(i, j) = values[i * this->nControlPoints + j]; // Not nice, but we checked the dimensions so this is safe.
297  }
298 
299  //ARMARX_DEBUG_S << this->controlNet << std::endl;
300  ARMARX_DEBUG_S << "done (rows: " << rows << ", Cols: " << lastCols << ", nDoF: " << this->nDoF << ", dim: " << this->dim << ")" << std::endl;
301  return true;
302  }
303 
304  void KBM::reset()
305  {
306  this->controlNet = Matrix::Zero(this->dim, this->nControlPoints);
307  }
308 
309  //ColumnVector crossproduct(const ColumnVector &a, const ColumnVector &b){
310  // ColumnVector result(3);
311  // result(1) = a(2)*b(3)-a(3)*b(2);
312  // result(2) = a(3)*b(1)-a(1)*b(3);
313  // result(3) = a(1)*b(2)-a(2)*b(1);
314  // return result;
315  //}
316 
317  Matrix KBM::predict(const Matrix& proprioception, int dim) const
318  {
319  if (dim == 0)
320  {
321  return controlNet * createSLE(proprioception);
322  }
323  else
324  {
325  //e = bez.controlNet * (blossom(bez,proprioception,dim,-bez.spreadAngle,proprioception(dim))-blossom(bez,proprioception,dim,bez.spreadAngle,proprioception(dim))#
326  //e2 = bez.controlNet * (blossom(bez,proprioception,dim,-bez.spreadAngle,proprioception(dim)+diff)-blossom(bez,proprioception,dim,bez.spreadAngle,proprioception(#
327  //e = cross(e1,e2);
328  //e = cross(e3,e1);
329  //frame(:3,:) = [e1/norm(e1) e2/norm(e2) e3/norm(e3) o];))))
330  Vector o = controlNet * createSLE(proprioception);
331  Vector3 e1 = this->controlNet * (createSLE(proprioception, dim, (-1) * this->spreadAngles[dim], proprioception(dim, 0)) -
332  createSLE(proprioception, dim, this->spreadAngles[dim], proprioception(dim, 0)));
333  Real offset = 0.1; // pi/6 // Magic number: Small offset that generates another vector to define the plane.
334  Vector3 e2 = this->controlNet * (createSLE(proprioception, dim, (-1) * this->spreadAngles[dim], proprioception(dim, 0) + offset) -
335  createSLE(proprioception, dim, this->spreadAngles[dim], proprioception(dim, 0) + offset));
336  //ARMARX_DEBUG_S << e1 << e2 << "e1/2";
337  Vector3 e3;
338  e3 = e1.cross(e2);
339  e2 = e3.cross(e1);
340  Matrix result = Matrix::Zero(4, 4);
341  //ARMARX_DEBUG_S << e1 << ",\n" << e2 << ",\n" <<e3 << ",\n" <<o;
342  result.block(0, 0, 3, 1) = e1 / e1.norm();
343  result.block(0, 1, 3, 1) = e2 / e2.norm();
344  result.block(0, 2, 3, 1) = e3 / e3.norm();
345  result.block(0, 3, 3, 1) = o;
346  result(3, 3) = 1.0f;
347  return result;
348  }
349  }
350 
352  {
353  for (int i = 0; i < this->nDoF; i++)
354  {
355 
356  }
357  }
358 
359  Vector KBM::getPartialDerivative(const Vector& proprioception, int iDoF) const
360  {
361 
362  Vector SLEa = this->createSLE(proprioception, iDoF, this->spreadAngles[iDoF], proprioception[iDoF], false);
363  Vector SLEb = this->createSLE(proprioception, iDoF, (-1) * this->spreadAngles[iDoF], proprioception[iDoF], false);
364  Vector SLEc = this->createSLE(proprioception, -1, 0.0f, 0.0f, false);
365 
366  Vector SLE = ((SLEa - SLEb) * SLEc.sum() - (SLEa.sum() - SLEb.sum()) * SLEc) / (SLEc.sum() * SLEc.sum());
367  return this->controlNet * SLE / cos(proprioception[iDoF] / 2.0f) / cos(proprioception[iDoF] / 2.0f) / 2 / tan(this->spreadAngles[iDoF] / 2.0f);
368  }
369 
370  Matrix KBM::getJacobian(const Vector& proprioception) const
371  {
372  Matrix Jacobian(this->dim, this->nDoF);
373 
374  for (int iDoF = 0; iDoF < this->nDoF; iDoF++)
375  {
376  Jacobian.col(iDoF) = this->getPartialDerivative(proprioception, iDoF);
377  }
378 
379  return Jacobian;
380  }
381 
383  {
384  return this->spreadAngles;
385  }
386 
388  {
389  return this->center;
390  }
391 
392  int KBM::getNDoF() const
393  {
394  return this->nDoF;
395  }
396 
397  int KBM::getNDim() const
398  {
399  return this->dim;
400  }
401 
403  {
404  return controlNet;
405  }
406 
407  KBM_ptr KBM::createKBM(int nDoF, int dim, const Vector& center, const Vector& spreadAngles, const Matrix& controlNet)
408  {
409  KBM_ptr kbm(new KBM(nDoF, dim, 0.0f));
410  kbm->controlNet = controlNet;
411  kbm->spreadAngles = spreadAngles;
412  kbm->center = center;
413  return kbm;
414  }
415 
416 
417 
418  void KBM::store(std::string fileName)
419  {
420  // ARMARX_DEBUG_S << "Storing to " << fileName << " ... " << std::flush;
421  std::ofstream file(fileName.c_str(), std::ofstream::trunc);
422  file << std::scientific << std::fixed << std::setprecision(std::numeric_limits<double>::digits);
423 
424  for (int iDim = 0; iDim < this->dim; iDim++)
425  {
426  for (int iControlPoints = 0; iControlPoints < this->nControlPoints - 1; iControlPoints++)
427  {
428  file << this->controlNet(iDim, iControlPoints) << ",";
429  // ARMARX_DEBUG_S << this->controlNet(iDim,iControlPoints) << ",";
430  }
431 
432  file << this->controlNet(iDim, nControlPoints - 1) << std::endl;
433  // ARMARX_DEBUG_S << this->controlNet(iDim,nControlPoints-1) << std::endl;
434  }
435 
436  file.close();
437  // ARMARX_DEBUG_S << "Done." << std::endl;
438  }
439 
440  KBM_ptr KBM::createFromVirtualRobot(VirtualRobot::KinematicChainPtr chain, VirtualRobot::SceneObjectPtr FoR, const Vector& spreadAngles, bool useOrientation)
441  {
442  Matrix controlNet;
443 
444  if (!useOrientation)
445  {
446  controlNet = FoR->toLocalCoordinateSystem(chain->getTCP()->getGlobalPose()).block(0, 3, 4, 1).cast<Real>();
447  }
448  else
449  {
450  controlNet.resize(12, 1);
451  Eigen::Transform<float, 3, Eigen::Affine> axis_x(Eigen::Translation<float, 3>(1.0, 0.0, 0.0));
452  Eigen::Transform<float, 3, Eigen::Affine> axis_y(Eigen::Translation<float, 3>(0.0, 1.0, 0.0));
453  controlNet.block(0, 0, 4, 1) = FoR->toLocalCoordinateSystem(chain->getTCP()->getGlobalPose()).block(0, 3, 4, 1).cast<Real>();
454  controlNet.block(4, 0, 4, 1) = FoR->toLocalCoordinateSystem(chain->getTCP()->getGlobalPose() * axis_x.matrix()).block(0, 3, 4, 1).cast<Real>();
455  controlNet.block(8, 0, 4, 1) = FoR->toLocalCoordinateSystem(chain->getTCP()->getGlobalPose() * axis_y.matrix()).block(0, 3, 4, 1).cast<Real>();
456  }
457 
458  for (int i = chain->getSize() - 1; i >= 0; --i)
459  {
460  Matrix temp = controlNet;
461  controlNet.resize(controlNet.rows(), controlNet.cols() * 3);
462 
463  VirtualRobot::RobotNodeRevolute* node = dynamic_cast<VirtualRobot::RobotNodeRevolute*>(chain->getNode(i).get());
464  Vector3 axis = node->getJointRotationAxisInJointCoordSystem().cast<Real>();
465  Matrix4 toLocal, toGlobal;
466  toLocal = node->getGlobalPose().inverse().cast<Real>();
467  toGlobal = node->getGlobalPose().cast<Real>();
468  Real factor = 1.0f / cos(spreadAngles(i));
469  Eigen::Transform<Real, 3, Eigen::Affine> rotation(Eigen::AngleAxis<Real>(spreadAngles(i), axis));
470  Eigen::Transform<Real, 3, Eigen::Affine> scaling;
471  Vector3 diag = axis + factor * (Vector3::Constant(1.0f) - axis);
472  scaling = Eigen::Scaling(diag);
473 
474  for (int i = 0; i < temp.rows() / 4; i++)
475  {
476  controlNet.block(i * 4, 0, 4, temp.cols()) = toGlobal * rotation.inverse() * toLocal * temp.block(i * 4, 0, 4, temp.cols());
477  controlNet.block(i * 4, temp.cols(), 4, temp.cols()) = toGlobal * scaling * toLocal * temp.block(i * 4, 0, 4, temp.cols());
478  controlNet.block(i * 4, 2 * temp.cols(), 4, temp.cols()) = toGlobal * rotation * toLocal * temp.block(i * 4, 0, 4, temp.cols());
479  }
480 
481  }
482 
483  Vector center = Vector::Constant(chain->getSize(), 0.0f);
484 
485  if (!useOrientation)
486  {
487  return KBM::createKBM(chain->getSize(), 3, center, spreadAngles, controlNet.block(0, 0, 3, controlNet.cols()));
488  }
489  else
490  {
491  Matrix reducedControlNet(9, controlNet.cols());
492  reducedControlNet.block(0, 0, 3, controlNet.cols()) = controlNet.block(0, 0, 3, controlNet.cols());
493  reducedControlNet.block(3, 0, 3, controlNet.cols()) = controlNet.block(4, 0, 3, controlNet.cols());
494  reducedControlNet.block(6, 0, 3, controlNet.cols()) = controlNet.block(8, 0, 3, controlNet.cols());
495  return KBM::createKBM(chain->getSize(), 9, center, spreadAngles, reducedControlNet);
496  }
497  }
498 
499 
500 }
kbm.h
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
memoryx::KBM::Models::KBM::batch
void batch(const Matrix &proprioception, const Matrix &shape, Optimization method=KBM::STANDARD, Real threshold=0.0)
Implements the batch learning algorithm.
Definition: kbm.cpp:200
memoryx::KBM::Models::KBM::restore
bool restore(std::string fileName="")
loads a KBM with same input/output dimensions from disk (comma-separated values).
Definition: kbm.cpp:248
memoryx::KBM::Models::KBM::getSpreadAngles
Vector getSpreadAngles() const
Return the spread angles.
Definition: kbm.cpp:382
memoryx::KBM::Models::KBM_ptr
std::shared_ptr< KBM > KBM_ptr
Definition: kbm.h:68
memoryx::KBM::Models::KBM::predict
Matrix predict(const Matrix &proprioception, int dim=0) const
Predicts a value of the FK given a set of joint angles.
Definition: kbm.cpp:317
memoryx::KBM::Models::KBM::KBM
KBM(const KBM &other)
Copy constructor.
Definition: kbm.cpp:30
memoryx::KBM::Models::KBM::getNDim
int getNDim() const
Definition: kbm.cpp:397
memoryx::KBM::Matrix
Eigen::MatrixXd Matrix
Definition: kbm.h:38
memoryx::KBM::Models::KBM::PLS
@ PLS
Definition: kbm.h:86
memoryx::KBM::Models::KBM::getControlNet
Matrix getControlNet() const
Definition: kbm.cpp:402
memoryx::KBM::Models::KBM::ErrorValuesType::IQR
Real IQR
Inerquartile range (50% of all errors): .
Definition: kbm.h:115
memoryx::KBM::Models::KBM::ErrorValuesType::STD
Real STD
Standard deviation.
Definition: kbm.h:111
memoryx::KBM::Models::KBM::ErrorValuesType::Mean
Real Mean
Mean error.
Definition: kbm.h:109
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
memoryx::KBM::Models::KBM::getJacobian
Matrix getJacobian(const Vector &proprioception) const
Computes the partial derivative with respect to a configuration.
Definition: kbm.cpp:370
memoryx::KBM::Vector
Eigen::VectorXd Vector
Definition: kbm.h:39
memoryx::KBM::Models::KBM::ErrorValuesType
Return type of the evaluation (KBM::getErrors()) function providing insight in the statistics of the ...
Definition: kbm.h:106
memoryx::KBM::Models::operator<<
std::ostream & operator<<(std::ostream &os, const KBM::ErrorValuesType &et)
Definition: kbm.cpp:24
memoryx::KBM::Models::KBM::reset
void reset()
Forgets all previously learned samples.
Definition: kbm.cpp:304
project
std::string project
Definition: VisualizationRobot.cpp:82
memoryx::KBM::Models::KBM::ErrorValuesType::Median
Real Median
Median of the error.
Definition: kbm.h:113
memoryx::KBM::Models::KBM::createFromVirtualRobot
static KBM_ptr createFromVirtualRobot(VirtualRobot::KinematicChainPtr chain, VirtualRobot::SceneObjectPtr FoR, const Vector &spreadAngles, bool useOrientation=false)
Definition: kbm.cpp:440
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
ARMARX_DEBUG_S
#define ARMARX_DEBUG_S
Definition: Logging.h:198
memoryx::KBM::Models::KBM::createKBM
static KBM_ptr createKBM(int nDoF, int dim, const Vector &center, const Vector &spreadAngles, const Matrix &controlNet)
Factory methods.
Definition: kbm.cpp:407
armarx::ValueToString
std::string ValueToString(const T &value)
Definition: StringHelpers.h:58
memoryx::KBM::Models::KBM::getNDoF
int getNDoF() const
Get the number of degrees of freedom (DoF).
Definition: kbm.cpp:392
memoryx::KBM::Models::KBM::STANDARD
@ STANDARD
Definition: kbm.h:85
memoryx::KBM::Models
Where the model representation for a Body Schema (especially the Kinematic BĀ“ezier Maps) reside.
Definition: inverse.cpp:19
armarx::armem::locations::toGlobal
void toGlobal(armarx::navigation::location::arondto::Location &framedLoc, const Eigen::Matrix4f &framedToGlobal)
Definition: frame_conversions.h:14
memoryx::KBM::Matrix4
Eigen::Matrix4d Matrix4
Definition: kbm.h:40
memoryx::KBM::Models::KBM::ErrorValuesType::Max
Real Max
Maximal error.
Definition: kbm.h:117
memoryx::KBM::Models::KBM::Optimization
Optimization
Enum for the preferred optimization method during batch learning.
Definition: kbm.h:83
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:67
memoryx::KBM::Models::KBM::online
void online(const Matrix &proprioception, const Matrix &shape, Real learnRate=1.0)
Implements the online learning algorithm.
Definition: kbm.cpp:221
ExpressionException.h
memoryx::KBM::Models::KBM::store
void store(std::string fileName="")
stores the current KBM as a comma-separated matrix to disk.
Definition: kbm.cpp:418
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
memoryx::KBM::Models::KBM
The Kinematic B\'ezier Maps.
Definition: kbm.h:78
scene3D::SceneObjectPtr
boost::intrusive_ptr< SceneObject > SceneObjectPtr
Definition: PointerDefinitions.h:40
Eigen::Matrix
Definition: EigenForwardDeclarations.h:27
memoryx::KBM::Models::KBM::getErrors
ErrorValuesType getErrors(const Matrix &proprioception, const Matrix &shape)
After learning this function gives various error measures over a given test set.online Proprioception...
Definition: kbm.cpp:57
Logging.h
memoryx::KBM::Models::KBM::differentiate
void differentiate()
Prepares the calculus of the derivatives. Has to be called alwayes after learning.
Definition: kbm.cpp:351
memoryx::KBM::Real
double Real
Type definition of the underlying Realing point type.
Definition: kbm.h:37
memoryx::KBM::Models::KBM::getCenter
Vector getCenter() const
Return the center.
Definition: kbm.cpp:387
memoryx::KBM::PLS::solve
Matrix KBM_IMPORT_EXPORT solve(const Matrix &input, const Matrix output, Real threshold)
Solves a linear system of equations using the partial least squares algorithm.
Definition: pls.cpp:32
memoryx::KBM::Models::KBM::getPartialDerivative
Vector getPartialDerivative(const Vector &proprioception, int iDoF) const
Computes a partial derivative with respect to a configuration and the specified degree of freedom.
Definition: kbm.cpp:359