void CStereoMatching::LowestLevelInitialMatch(cv::Mat image[], cv::Mat mask[], cv::Mat &disparity, bool IsZeroOne)
{
if (Verbose>=2)
printf("\t\tlowest level initial match starts...\n");
disparity = cv::Mat(m_data->m_LowestLevelSize, CV_16SC1, cv::Scalar(NOMATCH));
int YL, YR, XL, XR, XL1, XR1;
YL = margin[!IsZeroOne].YL;
YR = margin[!IsZeroOne].YR;
XL = margin[!IsZeroOne].XL;
XR = margin[!IsZeroOne].XR;
XL1 = margin[IsZeroOne].XL;
XR1 = margin[IsZeroOne].XR;
int window_size = 2*MatchBlockRadius+1;
int vec_size = square_(window_size)*3;
#pragma omp parallel for
for (int y=YL; y<=YR; y++)
{
uchar *p = mask[0].ptr<uchar>(y);
uchar *q = mask[1].ptr<uchar>(y);
short *s = disparity.ptr<short>(y);
uchar ** window_ptrL = new uchar *[window_size];
uchar ** window_ptrR = new uchar *[window_size];
for (int i=-MatchBlockRadius; i<=MatchBlockRadius; i++)
{
window_ptrL[i+MatchBlockRadius] = image[0].ptr<uchar>(y+i);
window_ptrR[i+MatchBlockRadius] = image[1].ptr<uchar>(y+i);
}
for (int x=XL; x<=XR; x++)
{
// mask
if (p[x] != 255)// || window_ptrL[MatchBlockRadius][XL*3] > 200 || window_ptrL[MatchBlockRadius][XL*3+1] > 200 || window_ptrL[MatchBlockRadius][XL*3] > 200)
continue;
arma::vec vecL(vec_size), vecR(vec_size);
double normL = m_data->WindowToVec(window_ptrL, x-MatchBlockRadius, window_size, vecL);
vecL /= normL;
short temp_i = -1;
double CurrentMaxValue = -1;
for (int iMatch=XL1; iMatch<=XR1; iMatch++)
{
if (q[iMatch] != 255)
continue;
double normR = m_data->WindowToVec(window_ptrR, iMatch-MatchBlockRadius, window_size, vecR);
double CurrentValue = arma::dot(vecL, vecR)/normR;
if (CurrentValue > CurrentMaxValue)
{
temp_i = iMatch;
CurrentMaxValue = CurrentValue;
}
}
if (temp_i != -1)
{
s[x] = short(temp_i - x);
}
}
delete [] window_ptrL;
delete [] window_ptrR;
}
}
void add_neighbourhood_to_hullPoints(pointVector& hullPoints, const Vertex_const_handle& vert, const double& tapeSize) {
Point_3 pnt = vert->point();
hullPoints.push_back(pnt); // Add vertex point
Nef_polyhedron::SVertex_const_iterator svcIt = vert->svertices_begin(), svcItEND = vert->svertices_end();
CGAL_For_all(svcIt,svcItEND) {
Vector_3 vecR(pnt,svcIt->target()->point());
Vector_3 vecRnew = vecR * tapeSize / std::sqrt(CGAL::to_double(vecR.squared_length()));
if ((vecR.squared_length()-OVERLAP_DIST_THRESHOLD) > vecRnew.squared_length())
hullPoints.push_back(pnt+vecRnew); // Add svertex neighbourhood point (tapesize away from vertex)
else
hullPoints.push_back(svcIt->target()->point());
}
//-----------------------------------------------------------------------------
// Name: FrameMove
// Desc:
//-----------------------------------------------------------------------------
HRESULT CMyD3DApplication::FrameMove()
{
HRESULT hr;
//
// Process keyboard input
//
D3DXVECTOR3 vecT(0.0f, 0.0f, 0.0f);
D3DXVECTOR3 vecR(0.0f, 0.0f, 0.0f);
if(m_bKey[VK_NUMPAD1] || m_bKey[VK_LEFT]) vecT.x -= 1.0f; // Slide Left
if(m_bKey[VK_NUMPAD3] || m_bKey[VK_RIGHT]) vecT.x += 1.0f; // Slide Right
if(m_bKey[VK_DOWN]) vecT.y -= 1.0f; // Slide Down
if(m_bKey[VK_UP]) vecT.y += 1.0f; // Slide Up
if(m_bKey['W']) vecT.z -= 2.0f; // Move Forward
if(m_bKey['S']) vecT.z += 2.0f; // Move Backward
if(m_bKey['A'] || m_bKey[VK_NUMPAD8]) vecR.x -= 1.0f; // Pitch Down
if(m_bKey['Z'] || m_bKey[VK_NUMPAD2]) vecR.x += 1.0f; // Pitch Up
if(m_bKey['E'] || m_bKey[VK_NUMPAD6]) vecR.y -= 1.0f; // Turn Right
if(m_bKey['Q'] || m_bKey[VK_NUMPAD4]) vecR.y += 1.0f; // Turn Left
if(m_bKey[VK_NUMPAD9]) vecR.z -= 2.0f; // Roll CW
if(m_bKey[VK_NUMPAD7]) vecR.z += 2.0f; // Roll CCW
m_vecVelocity = m_vecVelocity * 0.9f + vecT * 0.1f;
m_vecAngularVelocity = m_vecAngularVelocity * 0.9f + vecR * 0.1f;
//
// Update position and view matricies
//
D3DXMATRIX matT, matR;
D3DXQUATERNION qR;
vecT = m_vecVelocity * m_fElapsedTime * m_fSpeed;
vecR = m_vecAngularVelocity * m_fElapsedTime * m_fAngularSpeed;
D3DXMatrixTranslation(&matT, vecT.x, vecT.y, vecT.z);
D3DXMatrixMultiply(&m_matPosition, &matT, &m_matPosition);
D3DXQuaternionRotationYawPitchRoll(&qR, vecR.y, vecR.x, vecR.z);
D3DXMatrixRotationQuaternion(&matR, &qR);
D3DXMatrixMultiply(&m_matPosition, &matR, &m_matPosition);
D3DXMatrixInverse(&m_matView, NULL, &m_matPosition);
//
// Update simulation
//
if(!m_bPause && m_bDrawWater)
{
BOOL bCaustics = m_bDrawCaustics && m_pEffect->IsParameterUsed("tCAU");
D3DXVECTOR3 vecPos(m_matPosition._41, m_matPosition._42, m_matPosition._43);
D3DXVECTOR3 vecLight(0.0f, 1.0f, 0.0f);
m_Water.Update(vecPos, vecLight, bCaustics);
m_fTime += m_fSecsPerFrame;
if(bCaustics)
{
if(SUCCEEDED(m_pRenderToSurface->BeginScene(m_pCausticSurf, NULL)))
{
D3DXMATRIX matProj;
D3DXMATRIX matView;
D3DXMatrixOrthoRH(&matProj, 63.0f, 63.0f, 1.0f, 100.0f);
D3DXMatrixRotationX(&matView, 0.5f * D3DX_PI);
matView._43 = -50.0f;
m_pd3dDevice->SetTransform(D3DTS_PROJECTION, &matProj);
m_pd3dDevice->SetTransform(D3DTS_VIEW, &matView);
m_pd3dDevice->Clear(0, NULL, D3DCLEAR_TARGET, 0xff000000, 0.0f, 0);
m_pd3dDevice->SetRenderState(D3DRS_ZENABLE, FALSE);
m_pd3dDevice->SetRenderState(D3DRS_CULLMODE, D3DCULL_NONE);
m_pd3dDevice->SetRenderState(D3DRS_ALPHABLENDENABLE, TRUE);
m_Water.DrawCaustics();
m_pd3dDevice->SetRenderState(D3DRS_ALPHABLENDENABLE, FALSE);
m_pd3dDevice->SetRenderState(D3DRS_CULLMODE, D3DCULL_CW);
m_pd3dDevice->SetRenderState(D3DRS_ZENABLE, TRUE);
m_pRenderToSurface->EndScene();
}
else
{
m_bDrawCaustics = FALSE;
m_pEffect->SetTexture("tCAU", NULL);
if(FAILED(hr = GetNextTechnique(0, FALSE)))
return hr;
}
}
}
return S_OK;
}
// input: src_disparity<double>
// output: dst_disparity<short>
void CStereoMatching::HighLevelInitialMatch(cv::Mat image[], cv::Mat mask[], cv::Mat &disparity, int Pyrm_depth, bool IsZeroOne)
{
if (Verbose>=2)
printf("\t\thigh level initial match starts...\n");
cv::Mat dst_disparity(image[0].size(), CV_16S, cv::Scalar(NOMATCH));
int YL, YR, XL, XR, XL1, XR1, YR1;
YL = margin[!IsZeroOne].YL;
YR = margin[!IsZeroOne].YR;
XL = margin[!IsZeroOne].XL;
XR = margin[!IsZeroOne].XR;
XL1 = margin[IsZeroOne].XL;
XR1 = margin[IsZeroOne].XR;
YR1 = margin[IsZeroOne].YR;
int window_size = 2*MatchBlockRadius+1;
int vec_size = square_(window_size)*3;
#pragma omp parallel for
for (int y=YL; y<=YR; y++)
{
uchar *p = mask[0].ptr<uchar>(y);
uchar *q = mask[1].ptr<uchar>(y);
uchar ** window_ptrL = new uchar *[window_size];
uchar ** window_ptrR = new uchar *[window_size];
for (int i=-MatchBlockRadius; i<=MatchBlockRadius; i++)
{
window_ptrL[i+MatchBlockRadius] = image[0].ptr<uchar>(y+i);
window_ptrR[i+MatchBlockRadius] = image[1].ptr<uchar>(y+i);
}
short *d = dst_disparity.ptr<short>(y);
double *s = disparity.ptr<double>(int((y+1)/2.0));//@@û¿´¶®MIN(int((y+1)/2.0), YR1)
int boundary_L = XL1;
int boundary_R = XR1;
for (int x=XL; x<=XR; x++)
{
// mask
if (p[x] != 255)// || window_ptrL[MatchBlockRadius][XL*3] > 200 || window_ptrL[MatchBlockRadius][XL*3+1] > 200 || window_ptrL[MatchBlockRadius][XL*3] > 200)
continue;
int temp2 = int((x+1)/2.0);//@@û¿´¶®MIN(int((x+1)/2.0), XR1>>1)
arma::vec vecL(vec_size), vecR(vec_size);
double normL = m_data->WindowToVec(window_ptrL, x-MatchBlockRadius, window_size, vecL);
vecL /= normL;
ushort temp_i;
double CurrentMaxValue = -1;
if (s[temp2] == NOMATCH)
{
for (int i=temp2+1; i<=XR>>1 ; i++)
{
if (s[i] != NOMATCH)
{
boundary_R = MIN(i + int(s[i]*2) + m_offset + 1, XR1);
break;
}
}
}
else
{
boundary_L = MAX(x + int(s[temp2]*2+0.5) - m_offset, XL1);
boundary_R = MIN(x + int(s[temp2]*2+0.5) + m_offset, XR1);
}
for (int iMatch=boundary_L; iMatch<=boundary_R; iMatch++)
{
if (q[iMatch] != 255)
continue;
double normR = m_data->WindowToVec(window_ptrR, iMatch-MatchBlockRadius, window_size, vecR);
double CurrentValue = arma::dot(vecL, vecR)/normR;
if (CurrentValue > CurrentMaxValue)
{
CurrentMaxValue = CurrentValue;
temp_i = iMatch;
}
}
if (CurrentMaxValue > -1)
d[x] = short(temp_i - x);
}
delete [] window_ptrL;
delete [] window_ptrR;
}
