void serialize(Archive & ar, ::cv::Mat_<T>& m, const unsigned int version) { if(Archive::is_loading::value == true) { int cols, rows; size_t elem_size, elem_type; ar & cols; ar & rows; ar & elem_size; ar & elem_type; m.create(rows, cols); size_t data_size = m.cols * m.rows * elem_size; ar & boost::serialization::make_array(m.ptr(), data_size); } else { size_t elem_size = m.elemSize(); size_t elem_type = m.type(); ar & m.cols; ar & m.rows; ar & elem_size; ar & elem_type; const size_t data_size = m.cols * m.rows * elem_size; ar & boost::serialization::make_array(m.ptr(), data_size); } }
int NewlyExtractAndComputeSubImageDaisyDescriptorsCost( daisy *desc, cv::Mat_<float> &costOut, const cv::Mat_<float> &descRef, float oy, float ox, int h, int w, float dy, float dx, float step, int ori) { float ssinOri, scosOri; ssinOri = step*sin((float)ori/180.0*M_PI); scosOri = step*cos((float)ori/180.0*M_PI); //printf("%f %f %f y = %f x = %f fl = %f %f\n", step, ssinOri, scosOri, oy, ox, dy, dx); cv::Matx23f tranMat(scosOri, -ssinOri, dx, ssinOri, scosOri, dy); int cy, cx; int py, px; py = oy; for (cy=0; cy<h; ++cy, ++py) { float *ptr = (float *)(descRef.ptr(cy*w)); px = ox; for (cx=0; cx<w; ++cx, ++px) { cv::Matx31f p0(px, py, 1.0); cv::Matx21f p1 = tranMat*p0; //cv::Matx21f p1(px+dx, py+dy); // printf("%f %f dx = %f dy = %f px = %d py = %d oy = %f ox = %f\n", p1.val[0], p1.val[1], dx, dy, px, py, oy, ox); costOut[cy][cx] = desc->ExtractAndCalculateTheDistanceFromNormalizedHistogram(ptr, p1.val[1], p1.val[0], ori); ptr += DAISY_FEATURE_LENGTH; } } return 1; }
void find_zeros(const cv::Mat_<T>& binary, std::vector<cv::Point> &idx) { assert(binary.cols > 0 && binary.rows > 0 && binary.channels() == 1 && binary.channels() == 1); const int M = binary.rows; const int N = binary.cols; for (int m = 0; m < M; ++m) { const T* bin_ptr = (T*) binary.ptr(m); for (int n = 0; n < N; ++n) { if (bin_ptr[n] == 0) idx.push_back(cv::Point(n,m)); } } }
int ExtractAndComputeSubImageDaisyDescriptorsCost( daisy *desc, cv::Mat_<float> &costOut, cv::Mat_<float> &descRef, float py, float px, float step, int ori, int h, int w, cv::Vec4f &tmpFl ) { //printf("here 0 %d %d py = %f px = %f\n", w, h, py, px); //descOut.create(w*h, DAISY_FEATURE_LENGTH); //printf("here 0-1\n"); //descOut.setTo(cv::Scalar(0.0)); //printf("here 0-2\n"); int iy, ix; float cy, cx, ssinOri, scosOri; ssinOri = step*sin((float)ori/180.0*M_PI); scosOri = step*cos((float)ori/180.0*M_PI); float ry, rx; // the first pos of each row ry = py; rx = px; //printf("here 1\n"); //float *thor = new float [DAISY_FEATURE_LENGTH]; cv::Matx23f tranMat(scosOri, -ssinOri, tmpFl[0], ssinOri, scosOri, tmpFl[1]); for (iy=0; iy<h; ++iy) { cy = ry; cx = rx; float *ptr = (float *)(descRef.ptr(iy*w)); //printf("here 2 %f %f %f %f\n", cy, cx, ssinOri, scosOri); for (ix=0; ix<w; ++ix) { //memset(thor, 0, sizeof(float)*desc->descriptor_size()); // desc->get_descriptor(std::min<double>(limH-1.0, std::max<double>(cy, 0.0)), std::min<double>(limW-1.0, std::max<double>(cx, 0.0)), ori, ptr); costOut[iy][ix] = desc->ExtractAndCalculateTheDistanceFromNormalizedHistogram(ptr, cy, cx, ori); //cv::Matx31f p0(px-tmpFl[0]+ix, py-tmpFl[1]+iy, 1.0); //cv::Matx21f p1 = tranMat*p0; //printf("iy = %d ix = %d, cy = %f cx = %f, ori = %d, p1 = %f %f, ptr = %p\n", iy, ix, cy, cx, ori, p1.val[1], p1.val[0], ptr); //printf("py = %f px = %f, fl = %f %f, diff = %f %f\n", py, px, tmpFl[0], tmpFl[1], p1.val[0]-cx, p1.val[1]-cy); //costOut[iy][ix] = desc->ExtractAndCalculateTheDistanceFromNormalizedHistogram(ptr, p1.val[1], p1.val[0], ori); ptr += DAISY_FEATURE_LENGTH; //printf("here 2-1\n"); //memcpy(ptr, thor, sizeof(float)*DAISY_FEATURE_LENGTH); //printf("here 2-2\n"); //printf("here 2-3\n"); cy = cy+ssinOri; cx = cx+scosOri; } ry = ry+scosOri; rx = rx-ssinOri; //printf("here 2\n"); } //delete [] thor; //printf("here 3\n"); return 1; }
inline void cvToCloudOrganized(const cv::Mat_<cv::Point3f>& points3d, pcl::PointCloud<PointT>& cloud, size_t width, size_t height, const cv::Mat& mask = cv::Mat()) { cloud.points.resize(width * height); cloud.width = width; cloud.height = height; for (size_t v = 0; v < height; ++v) { const float * begin = reinterpret_cast<const float*>(points3d.ptr(v));for (size_t u = 0; u < width; ++u) { PointT& p = cloud(u, v); p.x = *(begin++); p.y = *(begin++); p.z = *(begin++); } } }
int ExtractSubImageDaisyDescriptors(daisy *desc, cv::Mat_<float> &descOut, float py, float px, float step, int ori, int h, int w, float limH, float limW) { //printf("here 0 %d %d py = %f px = %f\n", w, h, py, px); descOut.create(w*h, DAISY_FEATURE_LENGTH); //printf("here 0-1\n"); descOut.setTo(cv::Scalar(0.0)); //printf("here 0-2\n"); int iy, ix; float cy, cx, ssinOri, scosOri; ssinOri = step*sin((float)ori/180.0*M_PI); scosOri = step*cos((float)ori/180.0*M_PI); float ry, rx; // the first pos of each row ry = py; rx = px; //printf("here 1\n"); //float *thor = new float [DAISY_FEATURE_LENGTH]; for (iy=0; iy<h; ++iy) { cy = ry; cx = rx; float *ptr = (float *)(descOut.ptr(iy*w)); //printf("here 2 %f %f %f %f\n", cy, cx, ssinOri, scosOri); for (ix=0; ix<w; ++ix) { //memset(thor, 0, sizeof(float)*desc->descriptor_size()); //desc->get_descriptor(std::min<double>(limH-1.0, std::max<double>(cy, 0.0)), std::min<double>(limW-1.0, std::max<double>(cx, 0.0)), ori, ptr); desc->ExtractDescriptorFromNormalizedHistogram(ptr, std::min<double>(limH-1.0, std::max<double>(cy, 0.0)), std::min<double>(limW-1.0, std::max<double>(cx, 0.0)), ori); ptr += DAISY_FEATURE_LENGTH; //printf("here 2-1\n"); //memcpy(ptr, thor, sizeof(float)*DAISY_FEATURE_LENGTH); //printf("here 2-2\n"); //printf("here 2-3\n"); cy = cy+ssinOri; cx = cx+scosOri; } ry = ry+scosOri; rx = rx-ssinOri; //printf("here 2\n"); } //delete [] thor; //printf("here 3\n"); return 1; }
void CostBoxFilter::TheBoxFilter( const cv::Mat_<float> &bIn, cv::Mat_<float> &bOut, int radius ) { int height, width, iy, ix; height = bIn.rows; width = bIn.cols; bOut.create(height, width); cv::Mat_<float> cumHorSum(height, width); float *horSum = new float [width]; for (iy=0; iy<height; ++iy) { float *bPtr = (float *)(bIn.ptr(iy)); float *hPtr = horSum; float s = 0.0f; for (ix=0; ix<width; ++ix) { //s += bIn[iy][ix]; //horSum[ix] = s; s += *bPtr++; *hPtr++ = s; } float *ptrR = horSum+radius; float *dPtr = (float *)(cumHorSum.ptr(iy)); for (ix=0; ix<=radius; ++ix) *dPtr++ = *ptrR++; // cumHorSum[iy][ix] = horSum[ix+radius]; float *ptrL = horSum; for (; ix<width-radius; ++ix) *dPtr++ = *ptrR++-*ptrL++; //cumHorSum[iy][ix] = horSum[ix+radius]-horSum[ix-radius-1]; --ptrR; for (; ix<width; ++ix) *dPtr++ = *ptrR-*ptrL++; //cumHorSum[iy][ix] = horSum[width-1]-horSum[ix-radius-1]; } const int W_FAC = width; float *colSum = new float [height]; for (ix=0; ix<width; ++ix) { float s = 0.0f; float *cuPtr = (float *)(cumHorSum.ptr(0))+ix; float *coPtr = colSum; for (iy=0; iy<height; ++iy, cuPtr+=W_FAC) { //s += cumHorSum[iy][ix]; //colSum[iy] = s; s += *cuPtr; *coPtr++ = s; } float *ptrD = colSum+radius; float *dPtr = (float *)(bOut.ptr(0))+ix; for (iy=0; iy<=radius; ++iy, dPtr+=W_FAC) //bOut[iy][ix] = colSum[iy+radius]; *dPtr = *ptrD++; float *ptrU = colSum; for (; iy<height-radius; ++iy, dPtr+=W_FAC) //bOut[iy][ix] = colSum[iy+radius]-colSum[iy-radius-1]; *dPtr = *ptrD++-*ptrU++; --ptrD; for (; iy<height; ++iy, dPtr+=W_FAC) // bOut[iy][ix] = colSum[height-1]-colSum[iy-radius-1]; *dPtr = *ptrD-*ptrU++; } delete [] horSum; delete [] colSum; }
void CFFilter::FastCLMF0FloatFilterPointer( const cv::Mat_<cv::Vec4b> &crMap, const cv::Mat_<float> &src, cv::Mat_<float> &dst ) { // qx_timer tt; // tt.start(); int iy, ix, width, height; width = crMap.cols; height = crMap.rows; cv::Mat_<float> cost = src; // first iteration cv::Mat_<float> crossHorSum(height, width); cv::Mat_<int> sizeHorSum(height, width); cv::Mat_<float> crossHorSumTranspose(width, height); cv::Mat_<int> sizeHorSumTranspose(width, height); cv::Mat_<cv::Vec4b> crMapTranspose(width, height); cv::transpose(crMap, crMapTranspose); float *horSum = new float [width+1]; float *rowSizeHorSum = new float [width+1]; float *verSum = new float [height+1]; int *colSizeVerSum = new int [height+1]; float *costPtr = (float *)(cost.ptr(0)); float *crossHorPtr = (float *)(crossHorSum.ptr(0)); int *sizeHorPtr = (int *)(sizeHorSum.ptr(0)); cv::Vec4b *crMapPtr = (cv::Vec4b *)(crMap.ptr(0)); for (iy=0; iy<height; ++iy) { float s = 0.0; float *horPtr = horSum; *horPtr++ = s; for (ix=0; ix<width; ++ix) { s += *costPtr++; *horPtr++ = s; } for (ix=0; ix<width; ++ix) { cv::Vec4b cross = *crMapPtr++; *crossHorPtr++ = horSum[ix+cross[2]+1]-horSum[ix-cross[0]]; *sizeHorPtr++ = cross[2]+cross[0]+1; } } cv::transpose(crossHorSum, crossHorSumTranspose); cv::transpose(sizeHorSum, sizeHorSumTranspose); crossHorPtr = (float *)(crossHorSumTranspose.ptr(0)); sizeHorPtr = (int *)(sizeHorSumTranspose.ptr(0)); cv::Mat_<float> crossVerSum(height, width); cv::Mat_<int> sizeVerSum(height, width); cv::Mat_<float> crossVerSumTranpose(width, height); cv::Mat_<int> sizeVerSumTranpose(width, height); float *crossVerPtr = (float *)(crossVerSumTranpose.ptr(0)); int *sizeVerPtr = (int *)(sizeVerSumTranpose.ptr(0)); crMapPtr = (cv::Vec4b *)(crMapTranspose.ptr(0)); const int W_FAC = width; for (ix=0; ix<width; ++ix) { float s = 0.0; int cs = 0; float *verPtr = verSum; int *colSizeVerPtr = colSizeVerSum; *verPtr++ = s; *colSizeVerPtr++ = cs; for (iy=0; iy<height; ++iy) { s += *crossHorPtr++; *verPtr++ =s; cs += *sizeHorPtr++; *colSizeVerPtr++ = cs; } for (iy=0; iy<height; ++iy) { cv::Vec4b cross = *crMapPtr++; *crossVerPtr++ = verSum[iy+cross[3]+1]-verSum[iy-cross[1]]; *sizeVerPtr++ = colSizeVerSum[iy+cross[3]+1]-colSizeVerSum[iy-cross[1]]; } } cv::transpose(crossVerSumTranpose, crossVerSum); cv::transpose(sizeVerSumTranpose, sizeVerSum); // second iteration crossVerPtr = (float *)(crossVerSum.ptr(0)); sizeVerPtr = (int *)(sizeVerSum.ptr(0)); crossHorPtr = (float *)(crossHorSum.ptr(0)); sizeHorPtr = (int *)(sizeHorSum.ptr(0)); crMapPtr = (cv::Vec4b *)(crMap.ptr(0)); for (iy=0; iy<height; ++iy) { float s = 0.0; int rs = 0; float *horPtr = horSum; float *rowSizeHorPtr = rowSizeHorSum; *horPtr++ = s; *rowSizeHorPtr++ = rs; for (ix=0; ix<width; ++ix) { s += *crossVerPtr++; *horPtr++ = s; rs += *sizeVerPtr++; *rowSizeHorPtr++ = rs; } for (ix=0; ix<width; ++ix) { cv::Vec4b cross = *crMapPtr++; *crossHorPtr++ = horSum[ix+cross[2]+1]-horSum[ix-cross[0]]; *sizeHorPtr++ = rowSizeHorSum[ix+cross[2]+1]-rowSizeHorSum[ix-cross[0]]; } } cv::transpose(crossHorSum, crossHorSumTranspose); cv::transpose(sizeHorSum, sizeHorSumTranspose); crossHorPtr = (float *)(crossHorSumTranspose.ptr(0)); sizeHorPtr = (int *)(sizeHorSumTranspose.ptr(0)); crossVerPtr = (float *)(crossVerSumTranpose.ptr(0)); sizeVerPtr = (int *)(sizeVerSumTranpose.ptr(0)); crMapPtr = (cv::Vec4b *)(crMapTranspose.ptr(0)); for (ix=0; ix<width; ++ix) { float s = 0.0; int cs = 0; float *verPtr = verSum; int *colSizeVerPtr = colSizeVerSum; *verPtr++ = s; *colSizeVerPtr++ = cs; for (iy=0; iy<height; ++iy) { s += *crossHorPtr++; *verPtr++ = s; cs += *sizeHorPtr++; *colSizeVerPtr++ = cs; } for (iy=0; iy<height; ++iy) { cv::Vec4b cross = *crMapPtr++; *crossVerPtr++ = verSum[iy+cross[3]+1]-verSum[iy-cross[1]]; *sizeVerPtr++ = colSizeVerSum[iy+cross[3]+1]-colSizeVerSum[iy-cross[1]]; } } //tt.time_display("Smoothing"); delete [] horSum; delete [] rowSizeHorSum; delete [] verSum; delete [] colSizeVerSum; cv::transpose(crossVerSumTranpose, crossVerSum); cv::transpose(sizeVerSumTranpose, sizeVerSum); dst.create(height, width); cv::Mat_<float> tmpDst = dst; float *pCrossVerSum = (float *)crossVerSum.ptr(0); int *pSizeVerSum = (int *)sizeVerSum.ptr(0); float *pTmpDst = (float *)tmpDst.ptr(0); for (iy=0; iy<height*width; ++iy) { (*pTmpDst++) = (*pCrossVerSum++)/(*pSizeVerSum++); } }