本文整理汇总了C++中ArSensorReading::getEncoderPoseTaken方法的典型用法代码示例。如果您正苦于以下问题：C++ ArSensorReading::getEncoderPoseTaken方法的具体用法？C++ ArSensorReading::getEncoderPoseTaken怎么用？C++ ArSensorReading::getEncoderPoseTaken使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类ArSensorReading的用法示例。
在下文中一共展示了ArSensorReading::getEncoderPoseTaken方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: adjustRawReadings
AREXPORT void ArRangeDevice::adjustRawReadings(bool interlaced)
{
std::list<ArSensorReading *>::iterator rawIt;
// make sure we have raw readings and a robot, and a delay to
// correct for (note that if we don't have a delay to correct for
// but have already been adjusting (ie someone changed the delay)
// we'll just keep adjusting)
if (myRawReadings == NULL || myRobot == NULL ||
(myAdjustedRawReadings == NULL && myRobot->getOdometryDelay() == 0))
return;
// if we don't already have a list then make one
if (myAdjustedRawReadings == NULL)
myAdjustedRawReadings = new std::list<ArSensorReading *>;
// if we've already adjusted these readings then don't do it again
if (myRawReadings->begin() != myRawReadings->end() &&
myRawReadings->front()->getAdjusted())
return;
std::list<ArSensorReading *>::iterator adjIt;
ArSensorReading *adjReading;
ArSensorReading *rawReading;
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)
//.........这里部分代码省略.........