void extend(Rec &rec) { ulen len=random.select(rec.mem.len,rec.mem.len+rec.mem.len/2); if( rec.extend(heap,len) ) stat.count(OpExtendDone); stat.count(OpExtend); }
void match::domatch(std::vector<SamePoint>& resultData) { const int nBlockSize = 512; int scale = 1; int nx1, ny1, nx2, ny2, nband1, nband2; uchar *pBuf = NULL; m_pImage->Open(_bstr_t(m_szPathNameL), modeRead); m_pImage->GetCols(&nx1); m_pImage->GetRows(&ny1); m_pImage->GetBandNum(&nband1); m_pImage->Close(); m_pImage->Open(_bstr_t(m_szPathNameR), modeRead); m_pImage->GetCols(&nx2); m_pImage->GetRows(&ny2); m_pImage->GetBandNum(&nband2); m_pImage->Close(); int mincr = /*max(max(max(nx1, ny1), nx2),ny2)*/(nx1+nx2+ny1+ny2)/4; while(mincr/scale >1024) { scale *= 2; } int nxsize1 = nx1/scale; int nysize1 = ny1/scale; int nxsize2 = nx2/scale; int nysize2 = ny2/scale; m_pImage->Open(_bstr_t(m_szPathNameL), modeRead); pBuf = new uchar[nxsize1*nysize1*nband1]; m_pImage->ReadImg(0, 0, nx1, ny1, pBuf, nxsize1, nysize1, nband1, 0, 0, nxsize1, nysize1, -1, 0); m_pImage->Close(); m_pImage->CreateImg(_bstr_t("templ.tif"), modeCreate, nxsize1, nysize1, Pixel_Byte, nband1, BIL, 0, 0, 1); m_pImage->WriteImg(0, 0, nxsize1, nysize1, pBuf, nxsize1, nysize1, nband1, 0, 0, nxsize1, nysize1, -1, 0); m_pImage->Close(); delete [] pBuf; m_pImage->Open(_bstr_t(m_szPathNameR), modeRead); pBuf = new uchar[nxsize2*nysize2*nband2]; m_pImage->ReadImg(0, 0, nx2, ny2, pBuf, nxsize2, nysize2, nband2, 0, 0, nxsize2, nysize2, -1, 0); m_pImage->Close(); m_pImage->CreateImg(_bstr_t("tempr.tif"), modeCreate, nxsize2, nysize2, Pixel_Byte, nband2, BIL, 0, 0, 1); m_pImage->WriteImg(0, 0, nxsize2, nysize2, pBuf, nxsize2, nysize2, nband2, 0, 0, nxsize2, nysize2, -1, 0); m_pImage->Close(); delete []pBuf; pBuf = NULL; sl.fetchFeatures("templ.tif"); sr.fetchFeatures("tempr.tif"); /*sl.fetchFeatures(m_szPathNameL); sr.fetchFeatures(m_szPathNameR);*/ Keypoint** nbrs; kd_node* kd_root; int nsize = sl.m_listKeyPoint.size(); int nsize2 = sr.m_listKeyPoint.size(); Keypoint* feat1 = (Keypoint*)malloc(nsize*sizeof(Keypoint)); std::list<Keypoint>::iterator temIte = sl.m_listKeyPoint.begin(); int i = 0; while(temIte != sl.m_listKeyPoint.end()) { feat1[i] = *temIte; ++i; ++temIte; } sl.m_listKeyPoint.clear(); Keypoint* feat2 = (Keypoint*)malloc(nsize2*sizeof(Keypoint)); temIte = sr.m_listKeyPoint.begin(); i = 0; while(temIte != sr.m_listKeyPoint.end()) { feat2[i] = *temIte; ++i; ++temIte; } sr.m_listKeyPoint.clear(); kd_root = kdtree_build(feat2, nsize2); int k = 0; double d0, d1; Keypoint* feat; int matchnum = 0; std::vector<SamePoint> sp; for (i = 0; i < nsize; ++i) { feat = feat1+i; k = kdtree_bbf_knn(kd_root, feat, 2, &nbrs, KDTREE_BBF_MAX_NN_CHKS); if (k == 2) { d0 = descr_dist_sq(feat, nbrs[0]); d1 = descr_dist_sq(feat, nbrs[1]); if (d0 < d1*NN_SQ_DIST_RATIO_THR) { sp.push_back(SamePoint(feat->dx, feat->dy, nbrs[0]->dx, nbrs[0]->dy)); ++matchnum; } } free(nbrs); } kdtree_release(kd_root); free(feat1); free(feat2); feat1 = NULL; feat2 = NULL; std::vector<double> matParameters; MatParamEstimator mpEstimator(5); int numForEstimate = 3; mpEstimator.leastSquaresEstimate(sp, matParameters); double usedData = Ransac<SamePoint, double>::compute(matParameters, &mpEstimator, sp, numForEstimate, resultData); sp.swap(std::vector<SamePoint>()); resultData.swap(std::vector<SamePoint>()); Pt lLeftTop(0, 0); Pt lRightBottom(nx1, ny1); double a = matParameters[0]; double b = matParameters[1]; double c = matParameters[2]*scale; double d = matParameters[3]; double e = matParameters[4]; double f = matParameters[5]*scale; std::list<Block> listDataBlock; for (int y = 0; y < ny1;) { if (y+nBlockSize < ny1) { for (int x = 0; x < nx1;) { if (x+nBlockSize < nx1) { Rec rect(a*x+b*y+c, a*(x+nBlockSize)+b*y+c, d*x+e*y+f, d*x+e*(y+nBlockSize)+f); if (rect.Intersects(Rec(0, nx2, 0, ny2))) { listDataBlock.push_back(Block(NULL, x, y, nBlockSize, nBlockSize)); } x += nBlockSize; } else { Rec rect(a*x+b*y+c, a*nx1+b*y+c, d*x+e*y+f, d*x+e*(y+nBlockSize)+f); if (rect.Intersects(Rec(0, nx2, 0, ny2))) { listDataBlock.push_back(Block(NULL, x, y, nx1-x, nBlockSize)); } x = nx1; } } y += nBlockSize; } else { for (int x = 0; x < nx1;) { if (x+nBlockSize < nx1) { Rec rect(a*x+b*y+c, a*(x+nBlockSize)+b*y+c, d*x+e*y+f, d*x+e*ny1+f); if (rect.Intersects(Rec(0, nx2, 0, ny2))) { listDataBlock.push_back(Block(NULL, x, y, nBlockSize, ny1-y)); } x += nBlockSize; } else { Rec rect(a*x+b*y+c, a*nx1+b*y+c, d*x+e*y+f, d*x+e*ny1+f); if (rect.Intersects(Rec(0, nx2, 0, ny2))) { listDataBlock.push_back(Block(NULL, x, y, nx1-x, ny1-y)); } x = nx1; } } y = ny1; } } int nBlockNumx = (nx2+nBlockSize-1)/nBlockSize; int nBlockNumy = (ny2+nBlockSize-1)/nBlockSize; int nBlockNum = nBlockNumx*nBlockNumy; std::vector<RBTree> vecKDTree(nBlockNum); std::list<Block>::iterator blockIte = listDataBlock.begin(); m_pImage->Open(_bstr_t(m_szPathNameL), modeRead); m_pImage2->Open(_bstr_t(m_szPathNameR), modeRead); int countblock = 0; std::list<SamePoint> listSP; std::vector<Keypoint> feature; std::vector<Keypoint> feature2; while(blockIte != listDataBlock.end()) { pBuf = new uchar[blockIte->nXSize*blockIte->nYSize*nband1]; m_pImage->ReadImg(blockIte->nXOrigin, blockIte->nYOrigin, blockIte->nXOrigin+blockIte->nXSize, blockIte->nYOrigin+blockIte->nYSize, pBuf, blockIte->nXSize, blockIte->nYSize, nband1, 0, 0, blockIte->nXSize, blockIte->nYSize, -1, 0); pixel_t* p = new pixel_t[blockIte->nXSize*blockIte->nYSize]; for (int y = 0; y < blockIte->nYSize; ++y) { for (int x = 0, m = 0; x < blockIte->nXSize*nband1; x += nband1, ++m) { double sum = 0; for (int n = 0; n < nband1; ++n) { sum += pBuf[y*blockIte->nXSize*nband1+x+n]; } p[y*blockIte->nXSize+m] = sum/(nband1*225.0); } } delete []pBuf; pBuf = NULL; sift(p, feature, blockIte->nXSize, blockIte->nYSize); p = NULL; std::vector<Keypoint>::iterator feaIte = feature.begin(); int count = 0; while(feaIte != feature.end()) { std::cout<<countblock<<"/"<<listDataBlock.size()<<":"<<count<<"/"<<feature.size()<<":"<<listSP.size()<<std::endl; ++count; feaIte->dx += blockIte->nXOrigin; feaIte->dy += blockIte->nYOrigin; int calx = int(feaIte->dx*a+feaIte->dy*b+c); int caly = int(feaIte->dx*d+feaIte->dy*e+f); int idx = calx/nBlockSize; int idy = caly/nBlockSize; if (idx >= nBlockNumx || idy >= nBlockNumy) { ++feaIte; continue; } int nBlockIndex = idy*nBlockNumx+idx; if (vecKDTree[nBlockIndex].num != 0 && vecKDTree[nBlockIndex].num < 50) { ++feaIte; continue; } if (vecKDTree[nBlockIndex].node == NULL) { int xo = idx*nBlockSize; int yo = idy*nBlockSize; int xsize = nBlockSize; int ysize = nBlockSize; if (idx == nBlockNumx-1) { xsize = nx2%nBlockSize; if (xsize == 0) { xsize = nBlockSize; } } if (idy == nBlockNumy-1) { ysize = ny2%nBlockSize; if (ysize == 0) { ysize = nBlockSize; } } pBuf = new uchar[xsize*ysize*nband2]; m_pImage2->ReadImg(xo, yo, xo+xsize, yo+ysize, pBuf, xsize, ysize, nband2, 0, 0, xsize, ysize, -1, 0); p = new pixel_t[xsize*ysize]; for(int y = 0; y < ysize; ++y) { for (int x = 0, m = 0; x < xsize*nband2; x += nband2, ++m) { double sum = 0; for (int n = 0; n < nband2; ++n) { sum += pBuf[y*xsize*nband2+x+n]; } p[y*xsize+m] = sum/(nband2*255.0); } } delete []pBuf; pBuf = NULL; sift(p, feature2, xsize, ysize); p = NULL; int nf2 = feature2.size(); vecKDTree[nBlockIndex].num = nf2; if (nf2 < 50) { ++feaIte; continue; } feat2 = (Keypoint*)malloc(nf2*sizeof(Keypoint)); std::vector<Keypoint>::iterator kIte2 = feature2.begin(); i = 0; while(kIte2 != feature2.end()) { kIte2->dx += xo; kIte2->dy += yo; feat2[i] = *kIte2; ++i; ++kIte2; } feature2.swap(std::vector<Keypoint>()); kd_root = kdtree_build(feat2, nf2); vecKDTree[nBlockIndex].node = kd_root; vecKDTree[nBlockIndex].feature = feat2; } k = kdtree_bbf_knn(vecKDTree[nBlockIndex].node, &(*feaIte), 2, &nbrs, KDTREE_BBF_MAX_NN_CHKS); if (k == 2) { d0 = descr_dist_sq(&(*feaIte), nbrs[0]); d1 = descr_dist_sq(&(*feaIte), nbrs[1]); if (d0 < d1*NN_SQ_DIST_RATIO_THR) { listSP.push_back(SamePoint(feaIte->dx, feaIte->dy, nbrs[0]->dx, nbrs[0]->dy)); } } free(nbrs); ++feaIte; } feature.swap(std::vector<Keypoint>()); std::vector<RBTree>::iterator kdIte = vecKDTree.begin(); while(kdIte != vecKDTree.end()) { if (kdIte->node != NULL) { free(kdIte->node); kdIte->node = NULL; free(kdIte->feature); kdIte->feature = NULL; } ++kdIte; } ++countblock; ++blockIte; } m_pImage->Close(); m_pImage2->Close(); std::vector<SamePoint> vecsp(std::make_move_iterator(std::begin(listSP)), std::make_move_iterator(std::end(listSP))); listSP.clear(); sp.swap(vecsp); mpEstimator.leastSquaresEstimate(sp, matParameters); if (sp.size() < 500) { usedData = Ransac<SamePoint, double>::compute(matParameters, &mpEstimator, sp, numForEstimate, resultData); } else { usedData = Ransac<SamePoint, double>::compute(matParameters, &mpEstimator, sp, numForEstimate, 0.5, 0.8, resultData); } std::cout<<usedData<<":"<<resultData.size()<<std::endl; sp.swap(std::vector<SamePoint>()); resultData.swap(std::vector<SamePoint>()); a = matParameters[0]; b = matParameters[1]; c = matParameters[2]; d = matParameters[3]; e = matParameters[4]; f = matParameters[5]; //mosaic Pt calrLeftTop; Pt calrRightBottom; calrLeftTop.x = lLeftTop.x*a+lLeftTop.y*b+c; calrLeftTop.y = lLeftTop.x*d+lLeftTop.y*e+f; calrRightBottom.x = lRightBottom.x*a+lRightBottom.y*b+c; calrRightBottom.y = lRightBottom.x*d+lRightBottom.y*e+f; Rec calRight(calrLeftTop.x, calrRightBottom.x, calrLeftTop.y, calrRightBottom.y); calRight.extend(); Rec Right(0, nx2, 0, ny2); Rec resultRec = Right.Union(calRight); Rec resultRectR = calRight.Intersected(Right); resultRectR.extend(); Pt callLeftTop; Pt callRightBottom; callLeftTop.y = (d/a*resultRectR.left-resultRectR.top+f-c*d/a)/(b*d/a-e); callLeftTop.x = (resultRectR.left-b*callLeftTop.y-c)/a; callRightBottom.y = (d/a*resultRectR.right-resultRectR.bottom+f-c*d/a)/(b*d/a-e); callRightBottom.x = (resultRectR.right-b*callRightBottom.y-c)/a; Rec resultRectL(callLeftTop.x, callRightBottom.x, callLeftTop.y, callRightBottom.y); resultRectL.extend(); m_pImage->CreateImg(_bstr_t("result.tif"), modeCreate, (int)resultRec.Width(), (int)resultRec.Height(), Pixel_Byte, nband1, BIL, 0, 0, 1); IImage* pImage = NULL; IImage* pImage2 = NULL; ::CoCreateInstance(CLSID_ImageDriver, NULL, CLSCTX_ALL, IID_IImage, (void**)&pImage); ::CoCreateInstance(CLSID_ImageDriver, NULL, CLSCTX_ALL, IID_IImage, (void**)&pImage2); pImage->Open(_bstr_t(m_szPathNameR), modeRead); pBuf = new uchar[nx2*nBlockSize*nband2]; for (int i = 0; i < ny2;) { if (i + nBlockSize < ny2) { pImage->ReadImg(0, i, nx2, i+nBlockSize, pBuf, nx2, nBlockSize, nband2, 0, 0, nx2, nBlockSize, -1, 0); m_pImage->WriteImg(int(0-resultRec.left), int(i-resultRec.top), int(nx2-resultRec.left), int(i+nBlockSize-resultRec.top), pBuf, nx2, nBlockSize, nband2, 0, 0, nx2, nBlockSize, -1, 0); i += nBlockSize; } else { pImage->ReadImg(0, i, nx2, ny2, pBuf, nx2, nBlockSize, nband2, 0, 0, nx2, ny2-i, -1, 0); m_pImage->WriteImg(int(0-resultRec.left), int(i-resultRec.top), int(nx2-resultRec.left), int(ny2-resultRec.top), pBuf, nx2, nBlockSize, nband2, 0, 0, nx2, ny2-i, -1, 0); i = ny2; } } pImage->Close(); delete []pBuf; pImage->Open(_bstr_t(m_szPathNameL), modeRead); pBuf = new uchar[nx1*nBlockSize*nband1]; for (int i = 0; i < calRight.Height();) { if (i+nBlockSize < calRight.Height()) { pImage->ReadImg(0, i, nx1, i+nBlockSize, pBuf, nx1, nBlockSize, nband1, 0, 0, nx1, nBlockSize, -1, 0); m_pImage->WriteImg(int(calRight.left-resultRec.left), int(calRight.top+i-resultRec.top), int(calRight.right-resultRec.left), int(calRight.top+i+nBlockSize-resultRec.top), pBuf, nx1, nBlockSize, nband1, 0, 0, nx1, nBlockSize, -1, 0); i += nBlockSize; } else { pImage->ReadImg(0, i, nx1, ny1, pBuf, nx1, nBlockSize, nband1, 0, 0, nx1, ny1-i, -1, 0); m_pImage->WriteImg(int(calRight.left-resultRec.left), int(calRight.top+i-resultRec.top), int(calRight.right-resultRec.left), int(calRight.bottom-resultRec.top), pBuf, nx1, nBlockSize, nband1, 0, 0, nx1, ny1-i, -1, 0); i = (int)calRight.Height(); } } pImage->Close(); delete []pBuf; pBuf = NULL; m_pImage->Close(); pImage->Open(_bstr_t(m_szPathNameL), modeRead); pImage2->Open(_bstr_t(m_szPathNameR), modeRead); m_pImage->Open(_bstr_t("result.tif"), modeReadWrite); pBuf = new uchar[nband1*(int)resultRectR.Width()*(int)resultRectR.Height()]; m_pImage->ReadImg(int(resultRectR.left-resultRec.left), int(resultRectR.top-resultRec.top), int(resultRectR.right-resultRec.left), int(resultRectR.bottom-resultRec.top), pBuf, (int)resultRectR.Width(), (int)resultRectR.Height(), nband1, 0, 0, (int)resultRectR.Width(), (int)resultRectR.Height(), -1, 0); uchar* pBufl = new uchar[nband1*(int)resultRectL.Width()*(int)resultRectL.Height()]; pImage->ReadImg((int)resultRectL.left, (int)resultRectL.top, (int)resultRectL.right, (int)resultRectL.bottom, pBufl, (int)resultRectL.Width(), (int) resultRectL.Height(), nband1, 0, 0, (int)resultRectL.Width(), (int)resultRectL.Height(), -1, 0); uchar* pBufr = new uchar[nband2*(int)resultRectR.Width()*(int)resultRectR.Height()]; pImage2->ReadImg((int)resultRectR.left, (int)resultRectR.top, (int)resultRectR.right, (int)resultRectR.bottom, pBufr, (int)resultRectR.Width(), (int)resultRectR.Height(), nband2, 0, 0, (int)resultRectR.Width(), (int)resultRectR.Height(), -1, 0); double lx, ly; for (int y = 0; y < (int)resultRectR.Height(); ++y) { for (int x = 0; x < (int)resultRectR.Width()*nband1; x += nband1) { ly = (d/a*(x+resultRectR.left)-(y+resultRectR.top)+f-c*d/a)/(b*d/a-e); lx = ((x+resultRectR.left)-b*ly-c)/a; if (ly < resultRectL.top || lx < resultRectL.left) { continue; } if (pBufl[(int)(ly-resultRectL.top)*(int)resultRectL.Width()*nband1+(int)(lx-resultRectL.left)] < 20 && pBufl[(int)(ly-resultRectL.top)*(int)resultRectL.Width()*nband1+(int)(lx-resultRectL.left)+1] < 20 && pBufl[(int)(ly-resultRectL.top)*(int)resultRectL.Width()*nband1+(int)(lx-resultRectL.left)+2] < 20) { for (int n = 0; n < nband2; ++n) { pBuf[y*(int)resultRectR.Width()*nband1+x+n] = pBufr[y*(int)resultRectR.Width()*nband2+x+n]; } } else if (pBufr[y*(int)resultRectR.Width()*nband1+x] < 20 && pBufr[y*(int)resultRectR.Width()*nband1+x+1]<20 && pBufr[y*(int)resultRectR.Width()*nband1+x+2]<20) { for (int n = 0; n < nband1; ++n) { pBuf[y*(int)resultRectR.Width()*nband1+x+n] = pBufl[(int)(ly-resultRectL.top)*(int)resultRectL.Width()*nband1+(int)(lx-resultRectL.left)+n]; } } else { for (int n = 0; n < nband1; ++n) { pBuf[y*(int)resultRectR.Width()*nband1+x+n] = (pBufl[(int)(ly-resultRectL.top)*(int)resultRectL.Width()*nband1+(int)(lx-resultRectL.left)+n] + pBufr[y*(int)resultRectR.Width()*nband2+x+n])/2; } } } } delete []pBufl; pBufl = NULL; delete []pBufr; pBufr = NULL; m_pImage->WriteImg(int(resultRectR.left-resultRec.left), int(resultRectR.top-resultRec.top), int(resultRectR.right-resultRec.left), int(resultRectR.bottom-resultRec.top), pBuf, (int)resultRectR.Width(), (int)resultRectR.Height(), nband1, 0, 0, (int)resultRectR.Width(), (int)resultRectR.Height(), -1, 0); delete []pBuf; pBuf = NULL; m_pImage->Close(); }