本文整理汇总了C++中Pose3D::conc方法的典型用法代码示例。如果您正苦于以下问题：C++ Pose3D::conc方法的具体用法？C++ Pose3D::conc怎么用？C++ Pose3D::conc使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Pose3D的用法示例。
在下文中一共展示了Pose3D::conc方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: update
void BallLocator::update(BallModel& ballModel)
{
DECLARE_DEBUG_DRAWING("module:BallLocator:Field", "drawingOnField");
DECLARE_DEBUG_DRAWING("module:BallLocator:Image", "drawingOnImage");
#ifdef TARGET_SIM
if(theFrameInfo.time <= lastFrameTime)
{
if(theFrameInfo.time < lastFrameTime)
init();
else
return;
}
#endif
// set extra members for seen ball to achieve the following behavior:
// if the ball has been seen in the last lower camera image, it will be ignored in
// the upper image to exclude any clutter in the robot's environment
ballWasSeenInThisFrame = theBallPercept.ballWasSeen;
if(ballWasSeenInThisFrame && theCameraInfo.camera == CameraInfo::upper && ballWasBeenSeenInLastLowerCameraImage)
ballWasSeenInThisFrame = false;
else if(theCameraInfo.camera == CameraInfo::lower)
ballWasBeenSeenInLastLowerCameraImage = ballWasSeenInThisFrame;
// perform prediction step for each filter
motionUpdate(ballModel);
// detect and handle collision with feet
Pose3D leftFoot = theTorsoMatrix;
Pose3D rightFoot = theTorsoMatrix;
leftFoot.conc(theRobotModel.limbs[MassCalibration::footLeft]).translate(footOffset.x, footOffset.y, -theRobotDimensions.heightLeg5Joint);
rightFoot.conc(theRobotModel.limbs[MassCalibration::footRight]).translate(footOffset.x, -footOffset.y, -theRobotDimensions.heightLeg5Joint);
Vector2<> leftFootCenter(leftFoot.translation.x, leftFoot.translation.y);
Vector2<> rightFootCenter(rightFoot.translation.x, rightFoot.translation.y);
COMPLEX_DRAWING("module:BallLocator:Image",
{
Vector2<int> leftCenterImage, leftTopImage, rightCenterImage, rightTopImage;
if(Geometry::calculatePointInImage(Vector3<>(leftFootCenter.x, leftFootCenter.y, 0), theCameraMatrix, theCameraInfo, leftCenterImage) &&
Geometry::calculatePointInImage(Vector3<>(leftFootCenter.x + footRadius, leftFootCenter.y, 0), theCameraMatrix, theCameraInfo, leftTopImage))
{
CIRCLE("module:BallLocator:Image", leftCenterImage.x, leftCenterImage.y, (leftCenterImage - leftTopImage).abs(), 0, Drawings::ps_solid, ColorRGBA(0, 0, 0), Drawings::bs_null, ColorRGBA());
}
if(Geometry::calculatePointInImage(Vector3<>(rightFootCenter.x, rightFootCenter.y, 0), theCameraMatrix, theCameraInfo, rightCenterImage) &&
Geometry::calculatePointInImage(Vector3<>(rightFootCenter.x + footRadius, rightFootCenter.y, 0), theCameraMatrix, theCameraInfo, rightTopImage))
{
CIRCLE("module:BallLocator:Image", rightCenterImage.x, rightCenterImage.y, (rightCenterImage - rightTopImage).abs(), 0, Drawings::ps_solid, ColorRGBA(0, 0, 0), Drawings::bs_null, ColorRGBA());
}
});