void CameraMatrixProvider::update(CameraMatrix& cameraMatrix)
{
cameraMatrix.computeCameraMatrix(theTorsoMatrix, theRobotCameraMatrix, theCameraCalibration);
cameraMatrix.isValid = theTorsoMatrix.isValid && theMotionInfo.isMotionStable &&
theFallDownState.state == FallDownState::upright &&
theFrameInfo.getTimeSince(theFilteredJointData.timeStamp) < 500 &&
theRobotInfo.penalty == PENALTY_NONE;
DECLARE_DEBUG_DRAWING("module:CameraMatrixProvider:calibrationHelper", "drawingOnImage", drawFieldLines(cameraMatrix););
void CameraMatrixProvider::update(CameraMatrix& cameraMatrix,
const TorsoMatrix& theTorsoMatrix,
const RobotCameraMatrix& theRobotCameraMatrix,
const CameraCalibration& theCameraCalibration,
const MotionInfo& theMotionInfo,
const FallDownState& theFallDownState,
const FrameInfo& theFrameInfo,
const FilteredJointData& theFilteredJointData,
const RobotInfo& theRobotInfo)
{
cameraMatrix.computeCameraMatrix(theTorsoMatrix, theRobotCameraMatrix, theCameraCalibration);
cameraMatrix.isValid = theTorsoMatrix.isValid && theMotionInfo.isMotionStable &&
theFallDownState.state == FallDownState::upright &&
theFrameInfo.getTimeSince(theFilteredJointData.timeStamp) < 500 &&
theRobotInfo.penalty == PENALTY_NONE;
//cout << "CameraMatrix z: " << cameraMatrix.translation.z << endl;
//DECLARE_DEBUG_DRAWING("module:CameraMatrixProvider:calibrationHelper", "drawingOnImage");
//COMPLEX_DRAWING("module:CameraMatrixProvider:calibrationHelper", drawFieldLines(cameraMatrix););
//TEAM_OUTPUT(idTeamCameraHeight, bin, cameraMatrix.translation.z);
}