/** * Orient head and eyes towards m_lookAt. */ void C_HL2MP_Player::UpdateLookAt( void ) { // head yaw if (m_headYawPoseParam < 0 || m_headPitchPoseParam < 0) return; // orient eyes m_viewtarget = m_vLookAtTarget; // blinking if (m_blinkTimer.IsElapsed()) { m_blinktoggle = !m_blinktoggle; m_blinkTimer.Start( RandomFloat( 1.5f, 4.0f ) ); } // Figure out where we want to look in world space. QAngle desiredAngles; Vector to = m_vLookAtTarget - EyePosition(); VectorAngles( to, desiredAngles ); // Figure out where our body is facing in world space. QAngle bodyAngles( 0, 0, 0 ); bodyAngles[YAW] = GetLocalAngles()[YAW]; float flBodyYawDiff = bodyAngles[YAW] - m_flLastBodyYaw; m_flLastBodyYaw = bodyAngles[YAW]; // Set the head's yaw. float desired = AngleNormalize( desiredAngles[YAW] - bodyAngles[YAW] ); desired = clamp( desired, m_headYawMin, m_headYawMax ); m_flCurrentHeadYaw = ApproachAngle( desired, m_flCurrentHeadYaw, 130 * gpGlobals->frametime ); // Counterrotate the head from the body rotation so it doesn't rotate past its target. m_flCurrentHeadYaw = AngleNormalize( m_flCurrentHeadYaw - flBodyYawDiff ); desired = clamp( desired, m_headYawMin, m_headYawMax ); SetPoseParameter( m_headYawPoseParam, m_flCurrentHeadYaw ); // Set the head's yaw. desired = AngleNormalize( desiredAngles[PITCH] ); desired = clamp( desired, m_headPitchMin, m_headPitchMax ); m_flCurrentHeadPitch = ApproachAngle( desired, m_flCurrentHeadPitch, 130 * gpGlobals->frametime ); m_flCurrentHeadPitch = AngleNormalize( m_flCurrentHeadPitch ); SetPoseParameter( m_headPitchPoseParam, m_flCurrentHeadPitch ); }
//Main tracking Algorithm void AnalysisModule::larvaFind(uchar * img, int imWidth, int imHeight, int frameInd){ input = cv::Mat(imHeight,imWidth,CV_8UC1,NULL); input.data = img; if(output.rows != imHeight | output.cols != imWidth) output.create(imHeight,imWidth,CV_8UC1); int nextInd = (index+1)%sampleInd.size(); //for Profiling tic(); sampleInd[nextInd] = frameInd; sampleTime[nextInd] = frameInd * frameIntervalMS; //On first image, automatically determine threshold level using the Otsu method // Minimizes within group variance of thresholded classes. Should land on the best boundary between backlight and larva if(index == -1) threshold = otsuThreshold(img,imWidth*imHeight); //Can speed this up by applying to a roi bounding box a bit larger than the previous one //Simple inverted binary threshold of the image cv::threshold(input,output,threshold,255,CV_THRESH_BINARY_INV); profile[0] = toctic(); //Detect Contours in the binary image cv::findContours(output,contours,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE); profile[1] = toctic(); //No contours detected if (contours.size() == 0) { return; } //find contour with largest perimeter length double maxLen = 0; int maxInd = -1; double cLen; for(int i=0; i<contours.size(); i++){ cLen = cv::arcLength(cv::Mat(contours[i]), false); if(cLen >= maxLen){ maxLen = cLen; maxInd = i; }; } //Check to make sure that the perimeter is a larva by simple size analysis //(larva should have a certain perimeter length at 8.1um/pixel) cLarva[nextInd] = contours[maxInd]; //calculate bounding box bBox[nextInd] = cv::boundingRect(cv::Mat(cLarva[nextInd])); profile[2] = toctic(); //Calculate fourier coefficients fourierDecompose(cLarva[nextInd],nFourier,fourier[nextInd]); centroid[nextInd] = cv::Point2f(fourier[nextInd][0][AX],fourier[nextInd][0][AY]); profile[3] = toctic(); //Reconstruct the estimated boundary fourierReconstruct(fourier[nextInd],cFit,fitRes); profile[4] = toctic(); //Calculate Curvature perimeterCurvature(cFit,curve,fitRes/8); profile[5] = toctic(); //Find head and tail based on curvature minimums (small angle = sharp region) findHeadTail(cFit,curve,headTail); head[nextInd] = headTail[0]; tail[nextInd] = headTail[1]; profile[6] = toctic(); //Calculate Skeleton skeletonCalc(cFit,skeleton,headTail,length[nextInd],neck[nextInd]); profile[7] = toctic(); //Calculate bearing and head angle to bearing bodyAngles(tailBearingAngle[nextInd], headToBodyAngle[nextInd], head[nextInd], neck[nextInd], tail[nextInd]); profile[8] = toctic(); //Capture stage position stagePos[nextInd] = cv::Point(gui->stageThread->xpos,gui->stageThread->ypos); //Keep track of entire history with a sample every 30 frames if((nextInd % 30) == 0){ fullTrack[(fullTrackInd+1)%fullTrack.size()].x = stagePos[nextInd].x/gui->stageThread->tickPerMM_X+centroid[nextInd].x*gui->camThread->umPerPixel/1000.0; fullTrack[(fullTrackInd+1)%fullTrack.size()].y = stagePos[nextInd].y/gui->stageThread->tickPerMM_Y+centroid[nextInd].y*gui->camThread->umPerPixel/1000.0; fullTrackStim[(fullTrackInd+1)%fullTrack.size()] = binStimMax; binStimMax = 0; //updated from stimThread fullTrackInd++; } //Calculate Velocities of head and tail calcVelocities(nextInd); //Spew out profiling info //for(int i=0; i<9; i++) qDebug("%d: %.4fms",i,profile[i]*1000); //qDebug("\n"); index++; };
/** * Orient head and eyes towards m_lookAt. */ void C_CHostage::UpdateLookAt( CStudioHdr *pStudioHdr ) { if (!m_isInit) { m_isInit = true; Initialize( ); } // head yaw if (m_headYawPoseParam < 0 || m_bodyYawPoseParam < 0 || m_headPitchPoseParam < 0) return; if (GetLeader()) { m_lookAt = GetLeader()->EyePosition(); } // orient eyes m_viewtarget = m_lookAt; // blinking if (m_blinkTimer.IsElapsed()) { m_blinktoggle = !m_blinktoggle; m_blinkTimer.Start( RandomFloat( 1.5f, 4.0f ) ); } // Figure out where we want to look in world space. QAngle desiredAngles; Vector to = m_lookAt - EyePosition(); VectorAngles( to, desiredAngles ); // Figure out where our body is facing in world space. float poseParams[MAXSTUDIOPOSEPARAM]; GetPoseParameters( pStudioHdr, poseParams ); QAngle bodyAngles( 0, 0, 0 ); bodyAngles[YAW] = GetRenderAngles()[YAW] + RemapVal( poseParams[m_bodyYawPoseParam], 0, 1, m_bodyYawMin, m_bodyYawMax ); float flBodyYawDiff = bodyAngles[YAW] - m_flLastBodyYaw; m_flLastBodyYaw = bodyAngles[YAW]; // Set the head's yaw. float desired = AngleNormalize( desiredAngles[YAW] - bodyAngles[YAW] ); desired = clamp( desired, m_headYawMin, m_headYawMax ); m_flCurrentHeadYaw = ApproachAngle( desired, m_flCurrentHeadYaw, HOSTAGE_HEAD_TURN_RATE * gpGlobals->frametime ); // Counterrotate the head from the body rotation so it doesn't rotate past its target. m_flCurrentHeadYaw = AngleNormalize( m_flCurrentHeadYaw - flBodyYawDiff ); desired = clamp( desired, m_headYawMin, m_headYawMax ); SetPoseParameter( pStudioHdr, m_headYawPoseParam, m_flCurrentHeadYaw ); // Set the head's yaw. desired = AngleNormalize( desiredAngles[PITCH] ); desired = clamp( desired, m_headPitchMin, m_headPitchMax ); m_flCurrentHeadPitch = ApproachAngle( desired, m_flCurrentHeadPitch, HOSTAGE_HEAD_TURN_RATE * gpGlobals->frametime ); m_flCurrentHeadPitch = AngleNormalize( m_flCurrentHeadPitch ); SetPoseParameter( pStudioHdr, m_headPitchPoseParam, m_flCurrentHeadPitch ); SetPoseParameter( pStudioHdr, "head_roll", 0.0f ); }