if (bStepByStep)  
{  
    std::cout << "Tracking: Waiting to the next step" << std::endl;  
    while(!mbStep && bStepByStep)  
        usleep(500);  
    mbStep = false;  
}