Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/feature/feature-point6d-relative.cpp	Lines:	0	146	0.0 %
Date:	2023-03-13 12:09:37	Branches:	0	406	0.0 %


/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* --- SOT --- */
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/pool.h>

#include <sot/core/debug.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/feature-point6d-relative.hh>
#include <sot/core/macros.hh>
#include <sot/core/matrix-geometry.hh>

using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
namespace dg = dynamicgraph;

#include <sot/core/factory.hh>
SOT_CORE_DISABLE_WARNING_PUSH
SOT_CORE_DISABLE_WARNING_DEPRECATED
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoint6dRelative,
                                   "FeaturePoint6dRelative");
SOT_CORE_DISABLE_WARNING_POP

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

FeaturePoint6dRelative::FeaturePoint6dRelative(const string &pointName)
    : FeaturePoint6d(pointName),
      positionReferenceSIN(NULL, "sotFeaturePoint6dRelative(" + name +
                                     ")::input(matrixHomo)::positionRef"),
      articularJacobianReferenceSIN(NULL, "sotFeaturePoint6dRelative(" + name +
                                              ")::input(matrix)::JqRef"),
      dotpositionSIN(NULL, "sotFeaturePoint6dRelative(" + name +
                               ")::input(matrixHomo)::dotposition"),
      dotpositionReferenceSIN(NULL,
                              "sotFeaturePoint6dRelative(" + name +
                                  ")::input(matrixHomo)::dotpositionRef") {
  jacobianSOUT.addDependency(positionReferenceSIN);
  jacobianSOUT.addDependency(articularJacobianReferenceSIN);

  errorSOUT.addDependency(positionReferenceSIN);

  errordotSOUT.addDependency(dotpositionReferenceSIN);

  signalRegistration(positionReferenceSIN << articularJacobianReferenceSIN);

  signalRegistration(dotpositionSIN << dotpositionReferenceSIN << errordotSOUT);

  errordotSOUT.setFunction(
      boost::bind(&FeaturePoint6dRelative::computeErrordot, this, _1, _2));
  initCommands();
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/** Compute the interaction matrix from a subset of
 * the possible features.
 */
Matrix &FeaturePoint6dRelative::computeJacobian(Matrix &Jres, int time) {
  sotDEBUG(15) << "# In {" << endl;

  const Matrix &Jq = articularJacobianSIN(time);
  const Matrix &JqRef = articularJacobianReferenceSIN(time);
  const MatrixHomogeneous &wMp = positionSIN(time);
  const MatrixHomogeneous &wMpref = positionReferenceSIN(time);

  const Matrix::Index cJ = Jq.cols();
  Matrix J(6, cJ);
  {
    MatrixHomogeneous pMw;
    pMw = wMp.inverse(Eigen::Affine);
    MatrixHomogeneous pMpref;
    pMpref = pMw * wMpref;

    MatrixTwist pVpref;
    buildFrom(pMpref, pVpref);
    J = pVpref * JqRef;
    J -= Jq;
  }

  const Flags &fl = selectionSIN(time);
  const int dim = dimensionSOUT(time);
  sotDEBUG(15) << "Dimension=" << dim << std::endl;
  Jres.resize(dim, cJ);

  unsigned int rJ = 0;
  for (unsigned int r = 0; r < 6; ++r)
    if (fl(r)) {
      for (unsigned int c = 0; c < cJ; ++c) Jres(rJ, c) = J(r, c);
      rJ++;
    }

  sotDEBUG(15) << "# Out }" << endl;
  return Jres;
}

/** Compute the error between two visual features from a subset
 * a the possible features.
 */
Vector &FeaturePoint6dRelative::computeError(Vector &error, int time) {
  sotDEBUGIN(15);

  //   /* TODO */
  //   error.resize(6); error.fill(.0);

  const MatrixHomogeneous &wMp = positionSIN(time);
  const MatrixHomogeneous &wMpref = positionReferenceSIN(time);

  MatrixHomogeneous pMw;
  pMw = wMp.inverse(Eigen::Affine);
  MatrixHomogeneous pMpref;
  pMpref = pMw * wMpref;

  MatrixHomogeneous Merr;
  try {
    if (isReferenceSet()) {
      // TODO: Deal with the case of FeaturePoint6dRelative reference without
      // dcast
      FeaturePoint6dRelative *sdes6d =
          dynamic_cast<FeaturePoint6dRelative *>(getReference());
      if (NULL != sdes6d) {
        const MatrixHomogeneous &wMp_des = sdes6d->positionSIN(time);
        const MatrixHomogeneous &wMpref_des =
            sdes6d->positionReferenceSIN(time);
        MatrixHomogeneous pMw_des;
        pMw_des = wMp_des.inverse(Eigen::Affine);
        MatrixHomogeneous pMpref_des;
        pMpref_des = pMw_des * wMpref_des;
        MatrixHomogeneous Minv;
        Minv = pMpref_des.inverse(Eigen::Affine);
        Merr = pMpref * Minv;
      } else {
        const MatrixHomogeneous &Mref = getReference()->positionSIN(time);
        MatrixHomogeneous Minv;
        Minv = Mref.inverse(Eigen::Affine);
        Merr = pMpref * Minv;
      }
    } else {
      Merr = pMpref;
    }
  } catch (...) {
    Merr = pMpref;
  }

  MatrixRotation Rerr;
  Rerr = Merr.linear();
  VectorUTheta rerr(Rerr);

  const Flags &fl = selectionSIN(time);
  error.resize(dimensionSOUT(time));
  unsigned int cursor = 0;
  for (unsigned int i = 0; i < 3; ++i) {
    if (fl(i)) error(cursor++) = Merr(i, 3);
  }
  for (unsigned int i = 0; i < 3; ++i) {
    if (fl(i + 3)) error(cursor++) = rerr.angle() * rerr.axis()(i);
  }

  sotDEBUGOUT(15);
  return error;
}

/** Compute the error between two visual features from a subset
 * a the possible features.
 *
 * This is computed by the desired feature.
 */
Vector &FeaturePoint6dRelative::computeErrorDot(Vector &errordot, int time) {
  sotDEBUGIN(15);

  //   /* TODO */
  //   error.resize(6); error.fill(.0);
  const MatrixHomogeneous &wMp = positionSIN(time);
  const MatrixHomogeneous &wMpref = positionReferenceSIN(time);
  const MatrixHomogeneous &wdMp = dotpositionSIN(time);
  const MatrixHomogeneous &wdMpref = dotpositionReferenceSIN(time);

  sotDEBUG(15) << "wdMp :" << wdMp << endl;
  sotDEBUG(15) << "wdMpref :" << wdMpref << endl;

  MatrixRotation dRerr;
  Vector dtrerr;

  try {
    MatrixRotation wRp;
    wRp = wMp.linear();
    MatrixRotation wRpref;
    wRpref = wMpref.linear();
    MatrixRotation wdRp;
    wdRp = wdMp.linear();
    MatrixRotation wdRpref;
    wdRpref = wdMpref.linear();

    Vector trp(3);
    trp = wMp.translation();
    Vector trpref(3);
    trpref = wMpref.translation();
    Vector trdp(3);
    trdp = wdMp.translation();
    Vector trdpref(3);
    trdpref = wdMpref.translation();

    sotDEBUG(15) << "Everything is extracted" << endl;
    MatrixRotation wdRpt, wRpt, op1, op2;
    wdRpt = wdRp.transpose();
    op1 = wdRpt * wRpref;
    wRpt = wRp.transpose();
    op2 = wRpt * wdRpref;
    dRerr = op1 + op2;

    sotDEBUG(15) << "dRerr" << dRerr << endl;
    Vector trtmp1(3), vop1(3), vop2(3);
    trtmp1 = trpref - trp;
    vop1 = wdRpt * trtmp1;
    trtmp1 = trdpref - trdp;
    vop2 = wRpt * trtmp1;
    dtrerr = vop1 - vop2;

    sotDEBUG(15) << "dtrerr" << dtrerr << endl;

  } catch (...) {
    sotDEBUG(15) << "You've got a problem with errordot." << std::endl;
  }

  VectorUTheta rerr(dRerr);

  const Flags &fl = selectionSIN(time);
  errordot.resize(dimensionSOUT(time));
  unsigned int cursor = 0;
  for (unsigned int i = 0; i < 3; ++i) {
    if (fl(i)) errordot(cursor++) = dtrerr(i);
  }
  for (unsigned int i = 0; i < 3; ++i) {
    if (fl(i + 3)) errordot(cursor++) = rerr.angle() * rerr.axis()(i);
  }

  sotDEBUGOUT(15);
  return errordot;
}

static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
void FeaturePoint6dRelative::display(std::ostream &os) const {
  os << "Point6dRelative <" << name << ">: (";

  try {
    const Flags &fl = selectionSIN.accessCopy();
    bool first = true;
    for (int i = 0; i < 6; ++i)
      if (fl(i)) {
        if (first) {
          first = false;
        } else {
          os << ",";
        }
        os << featureNames[i];
      }
    os << ") ";
  } catch (ExceptionAbstract e) {
    os << " selectSIN not set.";
  }
}

void FeaturePoint6dRelative::initCommands(void) {
  using namespace command;
  addCommand("initSdes", makeCommandVoid1(
                             *this, &FeaturePoint6dRelative::initSdes,
                             docCommandVoid1("Initialize the desired feature.",
                                             "string (desired feature name)")));
}

/* Initialise the reference value: set up the sdes signal of the current
 * feature, and freezes the position and position-reference of the desired
 * feature.
 */
void FeaturePoint6dRelative::initSdes(const std::string &nameSdes) {
  FeaturePoint6dRelative &sdes = dynamic_cast<FeaturePoint6dRelative &>(
      dg::PoolStorage::getInstance()->getEntity(nameSdes));

  setReference(&sdes);

  const int timeCurr = positionSIN.getTime() + 1;
  positionSIN.recompute(timeCurr);
  positionReferenceSIN.recompute(timeCurr);

  sdes.positionSIN.setConstant(positionSIN.accessCopy());
  sdes.positionReferenceSIN.setConstant(positionReferenceSIN.accessCopy());
}


Generated by: GCOVR (Version 4.2)

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 <dynamic-graph/all-commands.h>
16			#include <dynamic-graph/pool.h>
17
18			#include <sot/core/debug.hh>
19			#include <sot/core/exception-feature.hh>
20			#include <sot/core/feature-point6d-relative.hh>
21			#include <sot/core/macros.hh>
22			#include <sot/core/matrix-geometry.hh>
23
24			using namespace std;
25			using namespace dynamicgraph::sot;
26			using namespace dynamicgraph;
27			namespace dg = dynamicgraph;
28
29			#include <sot/core/factory.hh>
30			SOT_CORE_DISABLE_WARNING_PUSH
31			SOT_CORE_DISABLE_WARNING_DEPRECATED
32			DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoint6dRelative,
33			"FeaturePoint6dRelative");
34			SOT_CORE_DISABLE_WARNING_POP
35
36			/* --------------------------------------------------------------------- */
37			/* --- CLASS ----------------------------------------------------------- */
38			/* --------------------------------------------------------------------- */
39
40			FeaturePoint6dRelative::FeaturePoint6dRelative(const string &pointName)
41			: FeaturePoint6d(pointName),
42			positionReferenceSIN(NULL, "sotFeaturePoint6dRelative(" + name +
43			")::input(matrixHomo)::positionRef"),
44			articularJacobianReferenceSIN(NULL, "sotFeaturePoint6dRelative(" + name +
45			")::input(matrix)::JqRef"),
46			dotpositionSIN(NULL, "sotFeaturePoint6dRelative(" + name +
47			")::input(matrixHomo)::dotposition"),
48			dotpositionReferenceSIN(NULL,
49			"sotFeaturePoint6dRelative(" + name +
50			")::input(matrixHomo)::dotpositionRef") {
51			jacobianSOUT.addDependency(positionReferenceSIN);
52			jacobianSOUT.addDependency(articularJacobianReferenceSIN);
53
54			errorSOUT.addDependency(positionReferenceSIN);
55
56			errordotSOUT.addDependency(dotpositionReferenceSIN);
57
58			signalRegistration(positionReferenceSIN << articularJacobianReferenceSIN);
59
60			signalRegistration(dotpositionSIN << dotpositionReferenceSIN << errordotSOUT);
61
62			errordotSOUT.setFunction(
63			boost::bind(&FeaturePoint6dRelative::computeErrordot, this, _1, _2));
64			initCommands();
65			}
66
67			/* --------------------------------------------------------------------- */
68			/* --------------------------------------------------------------------- */
69			/* --------------------------------------------------------------------- */
70
71			/** Compute the interaction matrix from a subset of
72			* the possible features.
73			*/
74			Matrix &FeaturePoint6dRelative::computeJacobian(Matrix &Jres, int time) {
75			sotDEBUG(15) << "# In {" << endl;
76
77			const Matrix &Jq = articularJacobianSIN(time);
78			const Matrix &JqRef = articularJacobianReferenceSIN(time);
79			const MatrixHomogeneous &wMp = positionSIN(time);
80			const MatrixHomogeneous &wMpref = positionReferenceSIN(time);
81
82			const Matrix::Index cJ = Jq.cols();
83			Matrix J(6, cJ);
84			{
85			MatrixHomogeneous pMw;
86			pMw = wMp.inverse(Eigen::Affine);
87			MatrixHomogeneous pMpref;
88			pMpref = pMw * wMpref;
89
90			MatrixTwist pVpref;
91			buildFrom(pMpref, pVpref);
92			J = pVpref * JqRef;
93			J -= Jq;
94			}
95
96			const Flags &fl = selectionSIN(time);
97			const int dim = dimensionSOUT(time);
98			sotDEBUG(15) << "Dimension=" << dim << std::endl;
99			Jres.resize(dim, cJ);
100
101			unsigned int rJ = 0;
102			for (unsigned int r = 0; r < 6; ++r)
103			if (fl(r)) {
104			for (unsigned int c = 0; c < cJ; ++c) Jres(rJ, c) = J(r, c);
105			rJ++;
106			}
107
108			sotDEBUG(15) << "# Out }" << endl;
109			return Jres;
110			}
111
112			/** Compute the error between two visual features from a subset
113			* a the possible features.
114			*/
115			Vector &FeaturePoint6dRelative::computeError(Vector &error, int time) {
116			sotDEBUGIN(15);
117
118			// /* TODO */
119			// error.resize(6); error.fill(.0);
120
121			const MatrixHomogeneous &wMp = positionSIN(time);
122			const MatrixHomogeneous &wMpref = positionReferenceSIN(time);
123
124			MatrixHomogeneous pMw;
125			pMw = wMp.inverse(Eigen::Affine);
126			MatrixHomogeneous pMpref;
127			pMpref = pMw * wMpref;
128
129			MatrixHomogeneous Merr;
130			try {
131			if (isReferenceSet()) {
132			// TODO: Deal with the case of FeaturePoint6dRelative reference without
133			// dcast
134			FeaturePoint6dRelative *sdes6d =
135			dynamic_cast<FeaturePoint6dRelative *>(getReference());
136			if (NULL != sdes6d) {
137			const MatrixHomogeneous &wMp_des = sdes6d->positionSIN(time);
138			const MatrixHomogeneous &wMpref_des =
139			sdes6d->positionReferenceSIN(time);
140			MatrixHomogeneous pMw_des;
141			pMw_des = wMp_des.inverse(Eigen::Affine);
142			MatrixHomogeneous pMpref_des;
143			pMpref_des = pMw_des * wMpref_des;
144			MatrixHomogeneous Minv;
145			Minv = pMpref_des.inverse(Eigen::Affine);
146			Merr = pMpref * Minv;
147			} else {
148			const MatrixHomogeneous &Mref = getReference()->positionSIN(time);
149			MatrixHomogeneous Minv;
150			Minv = Mref.inverse(Eigen::Affine);
151			Merr = pMpref * Minv;
152			}
153			} else {
154			Merr = pMpref;
155			}
156			} catch (...) {
157			Merr = pMpref;
158			}
159
160			MatrixRotation Rerr;
161			Rerr = Merr.linear();
162			VectorUTheta rerr(Rerr);
163
164			const Flags &fl = selectionSIN(time);
165			error.resize(dimensionSOUT(time));
166			unsigned int cursor = 0;
167			for (unsigned int i = 0; i < 3; ++i) {
168			if (fl(i)) error(cursor++) = Merr(i, 3);
169			}
170			for (unsigned int i = 0; i < 3; ++i) {
171			if (fl(i + 3)) error(cursor++) = rerr.angle() * rerr.axis()(i);
172			}
173
174			sotDEBUGOUT(15);
175			return error;
176			}
177
178			/** Compute the error between two visual features from a subset
179			* a the possible features.
180			*
181			* This is computed by the desired feature.
182			*/
183			Vector &FeaturePoint6dRelative::computeErrorDot(Vector &errordot, int time) {
184			sotDEBUGIN(15);
185
186			// /* TODO */
187			// error.resize(6); error.fill(.0);
188			const MatrixHomogeneous &wMp = positionSIN(time);
189			const MatrixHomogeneous &wMpref = positionReferenceSIN(time);
190			const MatrixHomogeneous &wdMp = dotpositionSIN(time);
191			const MatrixHomogeneous &wdMpref = dotpositionReferenceSIN(time);
192
193			sotDEBUG(15) << "wdMp :" << wdMp << endl;
194			sotDEBUG(15) << "wdMpref :" << wdMpref << endl;
195
196			MatrixRotation dRerr;
197			Vector dtrerr;
198
199			try {
200			MatrixRotation wRp;
201			wRp = wMp.linear();
202			MatrixRotation wRpref;
203			wRpref = wMpref.linear();
204			MatrixRotation wdRp;
205			wdRp = wdMp.linear();
206			MatrixRotation wdRpref;
207			wdRpref = wdMpref.linear();
208
209			Vector trp(3);
210			trp = wMp.translation();
211			Vector trpref(3);
212			trpref = wMpref.translation();
213			Vector trdp(3);
214			trdp = wdMp.translation();
215			Vector trdpref(3);
216			trdpref = wdMpref.translation();
217
218			sotDEBUG(15) << "Everything is extracted" << endl;
219			MatrixRotation wdRpt, wRpt, op1, op2;
220			wdRpt = wdRp.transpose();
221			op1 = wdRpt * wRpref;
222			wRpt = wRp.transpose();
223			op2 = wRpt * wdRpref;
224			dRerr = op1 + op2;
225
226			sotDEBUG(15) << "dRerr" << dRerr << endl;
227			Vector trtmp1(3), vop1(3), vop2(3);
228			trtmp1 = trpref - trp;
229			vop1 = wdRpt * trtmp1;
230			trtmp1 = trdpref - trdp;
231			vop2 = wRpt * trtmp1;
232			dtrerr = vop1 - vop2;
233
234			sotDEBUG(15) << "dtrerr" << dtrerr << endl;
235
236			} catch (...) {
237			sotDEBUG(15) << "You've got a problem with errordot." << std::endl;
238			}
239
240			VectorUTheta rerr(dRerr);
241
242			const Flags &fl = selectionSIN(time);
243			errordot.resize(dimensionSOUT(time));
244			unsigned int cursor = 0;
245			for (unsigned int i = 0; i < 3; ++i) {
246			if (fl(i)) errordot(cursor++) = dtrerr(i);
247			}
248			for (unsigned int i = 0; i < 3; ++i) {
249			if (fl(i + 3)) errordot(cursor++) = rerr.angle() * rerr.axis()(i);
250			}
251
252			sotDEBUGOUT(15);
253			return errordot;
254			}
255
256			static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
257			void FeaturePoint6dRelative::display(std::ostream &os) const {
258			os << "Point6dRelative <" << name << ">: (";
259
260			try {
261			const Flags &fl = selectionSIN.accessCopy();
262			bool first = true;
263			for (int i = 0; i < 6; ++i)
264			if (fl(i)) {
265			if (first) {
266			first = false;
267			} else {
268			os << ",";
269			}
270			os << featureNames[i];
271			}
272			os << ") ";
273			} catch (ExceptionAbstract e) {
274			os << " selectSIN not set.";
275			}
276			}
277
278			void FeaturePoint6dRelative::initCommands(void) {
279			using namespace command;
280			addCommand("initSdes", makeCommandVoid1(
281			*this, &FeaturePoint6dRelative::initSdes,
282			docCommandVoid1("Initialize the desired feature.",
283			"string (desired feature name)")));
284			}
285
286			/* Initialise the reference value: set up the sdes signal of the current
287			* feature, and freezes the position and position-reference of the desired
288			* feature.
289			*/
290			void FeaturePoint6dRelative::initSdes(const std::string &nameSdes) {
291			FeaturePoint6dRelative &sdes = dynamic_cast<FeaturePoint6dRelative &>(
292			dg::PoolStorage::getInstance()->getEntity(nameSdes));
293
294			setReference(&sdes);
295
296			const int timeCurr = positionSIN.getTime() + 1;
297			positionSIN.recompute(timeCurr);
298			positionReferenceSIN.recompute(timeCurr);
299
300			sdes.positionSIN.setConstant(positionSIN.accessCopy());
301			sdes.positionReferenceSIN.setConstant(positionReferenceSIN.accessCopy());
302			}