本文整理汇总了C++中ArSensorReading::applyEncoderTransform方法的典型用法代码示例。如果您正苦于以下问题：C++ ArSensorReading::applyEncoderTransform方法的具体用法？C++ ArSensorReading::applyEncoderTransform怎么用？C++ ArSensorReading::applyEncoderTransform使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类ArSensorReading的用法示例。
在下文中一共展示了ArSensorReading::applyEncoderTransform方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: adjustRawReadings
//.........这里部分代码省略.........
ArTransform trans;
ArTransform encTrans;
ArTransform interlacedTrans;
ArTransform interlacedEncTrans;
bool first = true;
bool second = true;
int onReading;
for (rawIt = myRawReadings->begin(), adjIt = myAdjustedRawReadings->begin(),
onReading = 0;
rawIt != myRawReadings->end();
rawIt++, onReading++)
{
rawReading = (*rawIt);
if (adjIt != myAdjustedRawReadings->end())
{
adjReading = (*adjIt);
adjIt++;
}
else
{
adjReading = new ArSensorReading;
myAdjustedRawReadings->push_back(adjReading);
}
(*adjReading) = (*rawReading);
if (first || (interlaced && second))
{
ArPose origPose;
ArPose corPose;
ArPose origEncPose;
ArPose corEncPose;
ArTime corTime;
corTime = rawReading->getTimeTaken();
corTime.addMSec(-myRobot->getOdometryDelay());
if (myRobot->getPoseInterpPosition(corTime,
&corPose) == 1 &&
myRobot->getEncoderPoseInterpPosition(corTime,
&corEncPose) == 1)
{
origPose = rawReading->getPoseTaken();
origEncPose = rawReading->getEncoderPoseTaken();
/*
printf("Difference was %g %g %g (rotVel %.0f, rotvel/40 %g)\n",
origEncPose.getX() - corEncPose.getX(),
origEncPose.getY() - corEncPose.getY(),
origEncPose.getTh() - corEncPose.getTh(),
myRobot->getRotVel(), myRobot->getRotVel() / 40);
*/
if (first)
{
trans.setTransform(origPose, corPose);
encTrans.setTransform(origEncPose, corEncPose);
}
else if (interlaced && second)
{
interlacedTrans.setTransform(origPose, corPose);
interlacedEncTrans.setTransform(origEncPose, corEncPose);
}
}
else
{
//printf("Couldn't correct\n");
}
if (first)
first = false;
else if (interlaced && second)
second = false;
}
if (!interlaced && (onReading % 2) == 0)
{
adjReading->applyTransform(trans);
adjReading->applyEncoderTransform(encTrans);
}
else
{
adjReading->applyTransform(interlacedTrans);
adjReading->applyEncoderTransform(interlacedEncTrans);
}
/*
if (fabs(adjReading->getEncoderPoseTaken().getX() -
corEncPose.getX()) > 1 ||
fabs(adjReading->getEncoderPoseTaken().getY() -
corEncPose.getY()) > 1 ||
fabs(ArMath::subAngle(adjReading->getEncoderPoseTaken().getTh(),
corEncPose.getTh())) > .2)
printf("(%.0f %.0f %.0f) should be (%.0f %.0f %.0f)\n",
adjReading->getEncoderPoseTaken().getX(),
adjReading->getEncoderPoseTaken().getY(),
adjReading->getEncoderPoseTaken().getTh(),
corEncPose.getX(), corEncPose.getY(), corEncPose.getTh());
*/
adjReading->setAdjusted(true);
rawReading->setAdjusted(true);
}
}