whitebalancer.cpp

该程序实现FIRE足球机器人竞赛中的3：3比赛源码

// WhiteBalancer.cpp: implementation of the CWhiteBalancer class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "WhiteBalancer.h"
#include <float.h>

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

const int CWhiteBalancer::MAX_ITERATIONS = 24;
const double CWhiteBalancer::CBisection::THRESH_HIT = 0.5;
const double CWhiteBalancer::CBisection::THRESH_ACCEPT = 4.0;


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CWhiteBalancer::CWhiteBalancer() 
: m_fConverged(true),
  m_Min(0),
  m_Max(0),
  m_UB(0),
  m_VR(0)
{
}


CWhiteBalancer::~CWhiteBalancer()
{
}


void CWhiteBalancer::Init(int ub, int vr)
{
  m_nIterations = 0;
  m_fConverged = false;
  DCSVideoFormat videoFormat=DCS_Format7 ;
  DCSVideoMode videoMode= DCS_Mode0;
  DCSColorCode colorCode=DCSColor_Raw8;
  m_fBayerImage = colorCode == DCSColor_Mono8;
  if ( ! m_fBayerImage )
  {
	  m_Min =16;  //m_Bcam.WhiteBalance.Raw.Min();
	  m_Max =255; //m_Bcam.WhiteBalance.Raw.Max();
  }
  else
  {
    m_Min = 50;
    m_Max = 1000;
  }
  m_UB = ub;
  m_VR = vr;
  m_bestUB = ub;
  m_bestVR = vr;
  m_minError = DBL_MAX;
  m_VBisection.Init(m_Min, m_Max);
  m_UBisection.Init(m_Min, m_Max);
}

bool CWhiteBalancer::Next(PBYTE imgBuffer, const CSize& imgSize)
{

  if ( m_fConverged )
    return true;

  double meanr, meang, meanb;
  GetMeans(imgBuffer, imgSize, meanr, meang, meanb);

  double deltar = meanr - meang;
  double deltab = meanb - meang;

  double error  = deltar * deltar + deltab * deltab;
  TRACE("vr: %d, ub: %d  error: %f\n", m_VR, m_UB, error);

  if ( error < m_minError )
  {
    m_minError = error;
    m_bestUB = m_UB;
    m_bestVR = m_VR;
  }
  
  long oldUB = m_UB;
  long oldVR = m_VR;

  if ( m_fBayerImage )
  {
    m_UB = ( m_UB / 100.0 ) * ( meang / meanb ) * 100 + 0.5;
    if ( m_UB < m_Min ) m_UB = m_Min;
    if ( m_UB > m_Max ) m_UB = m_Max;
    m_VR = ( m_VR / 100.0 ) * ( meang / meanr ) * 100 + 0.5;
    if ( m_VR < m_Min ) m_VR = m_Min;
    if ( m_VR > m_Max ) m_VR = m_Max;
    m_fConverged = ( m_nIterations++ >= MAX_ITERATIONS || oldUB == m_UB && oldVR == m_VR  ) ;
  }
  else
  { 
    if ( m_nIterations == 0 )
    {
      m_UB = 0.5 * ( m_Min + m_Max );
      m_VR = 0.5 * ( m_Min + m_Max );
      m_nIterations++;
      return false;
    }
    boolean fConvergedVR = m_VBisection.Next(m_VR, meanr, meang, m_VR);
    boolean fConvergedUB = m_UBisection.Next(m_UB, meanb, meang, m_UB);
    
    m_fConverged = ( ( fConvergedUB && fConvergedVR) || m_nIterations++ >= MAX_ITERATIONS );
  }

  if ( m_fConverged )
  {
    // If there was an iteration yielding a better error, take these values instead
    if ( m_minError < error )
    {
      m_UB = m_bestUB;
      m_VR = m_bestVR;
      TRACE("Corrected: m_UB  %d, m_VR %d\n", m_UB, m_VR);
    }
  }

  return m_fConverged;

}

void CWhiteBalancer::GetMeans(PBYTE imgBuffer, const CSize& imgSize, double& meanr, double& meang, double& meanb )
{
  double sumr = 0;
  double sumb = 0;
  double sumg = 0;
  RGBTRIPLE* pRGB = (RGBTRIPLE*) imgBuffer;
  for ( int j = 0; j < imgSize.cy; ++j )
    for ( int i = 0; i < imgSize.cx; ++i )
    {
      sumr += pRGB->rgbtRed;
      sumb += pRGB->rgbtBlue;
      sumg += pRGB->rgbtGreen;
      pRGB++;
    }

  long nPixels = imgSize.cx * imgSize.cy;
  meanr = sumr / nPixels;
  meang = sumg / nPixels;
  meanb = sumb / nPixels;
  TRACE("meanr %f, meang %f, meanb %f\n", meanr, meang, meanb);
}