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++);
}
}