C++ (Cpp) odometryOffset3D Examples

Example #1

File: TorsoMatrixProvider.cpp Project: CheddarB/nbites

/*
void TorsoMatrixProvider::update(FilteredOdometryOffset& odometryOffset)
{
  Pose2DBH odometryOffset;

  if(lastTorsoMatrix.translation.z != 0.)
  {
    Pose3DBH odometryOffset3D(lastTorsoMatrix);
    odometryOffset3D.conc(theTorsoMatrixBH.offset);
    odometryOffset3D.conc(theTorsoMatrixBH.invert());

    odometryOffset.translation.x = odometryOffset3D.translation.x;
    odometryOffset.translation.y = odometryOffset3D.translation.y;
    odometryOffset.rotation = odometryOffset3D.rotation.getZAngle();
  }

  PLOT("module:TorsoMatrixProvider:odometryOffsetX", odometryOffset.translation.x);
  PLOT("module:TorsoMatrixProvider:odometryOffsetY", odometryOffset.translation.y);
  PLOT("module:TorsoMatrixProvider:odometryOffsetRotation", toDegrees(odometryOffset.rotation));

  (Pose3DBH&)lastTorsoMatrix = theTorsoMatrixBH;
}
*/
void TorsoMatrixProvider::update(OdometryDataBH& odometryData)
{
  Pose2DBH odometryOffset;

  if(lastTorsoMatrix.translation.z != 0.)
  {
    Pose3DBH odometryOffset3D(lastTorsoMatrix);
    odometryOffset3D.conc(theTorsoMatrixBH.offset);
    odometryOffset3D.conc(theTorsoMatrixBH.invert());

    odometryOffset.translation.x = odometryOffset3D.translation.x;
    odometryOffset.translation.y = odometryOffset3D.translation.y;
    odometryOffset.rotation = odometryOffset3D.rotation.getZAngle();
  }

  PLOT("module:TorsoMatrixProvider:odometryOffsetX", odometryOffset.translation.x);
  PLOT("module:TorsoMatrixProvider:odometryOffsetY", odometryOffset.translation.y);
  PLOT("module:TorsoMatrixProvider:odometryOffsetRotation", toDegrees(odometryOffset.rotation));

  odometryData += odometryOffset;

  (Pose3DBH&)lastTorsoMatrix = theTorsoMatrixBH;
}

Example #2

File: TorsoMatrixProvider.cpp Project: bhuman/BHumanCodeRelease

void TorsoMatrixProvider::update(OdometryData& odometryData)
{
  Pose2f odometryOffset;

  if(lastTorsoMatrix.translation.z() != 0.)
  {
    Pose3f odometryOffset3D(lastTorsoMatrix);
    odometryOffset3D.conc(theTorsoMatrix.offset);
    odometryOffset3D.conc(theTorsoMatrix.inverse());

    odometryOffset.translation.x() = odometryOffset3D.translation.x();
    odometryOffset.translation.y() = odometryOffset3D.translation.y();
    odometryOffset.rotation = odometryOffset3D.rotation.getZAngle();
  }

  PLOT("module:TorsoMatrixProvider:odometryOffsetX", odometryOffset.translation.x());
  PLOT("module:TorsoMatrixProvider:odometryOffsetY", odometryOffset.translation.y());
  PLOT("module:TorsoMatrixProvider:odometryOffsetRotation", odometryOffset.rotation.toDegrees());

  odometryData += odometryOffset;

  (Pose3f&)lastTorsoMatrix = theTorsoMatrix;
}