// Adapted from cv_show_angles
cv::Mat
displayQuantized (const cv::Mat& quantized)
{
cv::Mat color (quantized.size (), CV_8UC3);
for (int r = 0; r < quantized.rows; ++r)
{
const uchar* quant_r = quantized.ptr (r);
cv::Vec3b* color_r = color.ptr<cv::Vec3b> (r);
for (int c = 0; c < quantized.cols; ++c)
{
cv::Vec3b& bgr = color_r[c];
switch (quant_r[c])
{
case 0:
bgr[0] = 0;
bgr[1] = 0;
bgr[2] = 0;
break;
case 1:
bgr[0] = 55;
bgr[1] = 55;
bgr[2] = 55;
break;
case 2:
bgr[0] = 80;
bgr[1] = 80;
bgr[2] = 80;
break;
case 4:
bgr[0] = 105;
bgr[1] = 105;
bgr[2] = 105;
break;
case 8:
bgr[0] = 130;
bgr[1] = 130;
bgr[2] = 130;
break;
case 16:
bgr[0] = 155;
bgr[1] = 155;
bgr[2] = 155;
break;
case 32:
bgr[0] = 180;
bgr[1] = 180;
bgr[2] = 180;
break;
case 64:
bgr[0] = 205;
bgr[1] = 205;
bgr[2] = 205;
break;
case 128:
bgr[0] = 230;
bgr[1] = 230;
bgr[2] = 230;
break;
case 255:
bgr[0] = 0;
bgr[1] = 0;
bgr[2] = 255;
break;
default:
bgr[0] = 0;
bgr[1] = 255;
bgr[2] = 0;
break;
}
}
}
return color;
}
/**
* @function displayCam
* Draws the webcam image in window + detection info
*/
void displayCam(cv::Mat camImage)
{
//-- Save matrix
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
gluOrtho2D(0.0, windowWidth, 0.0, windowHeight);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
//-- Display Coordinates
if(bDisplayDetection)
{
//-- Coord text
std::stringstream sstm;
sstm << "(x,y,z) = (" << (int)glCamX << "," << (int)glCamY << "," << (int)glCamZ << ")";
std::string s = sstm.str();
//std::cout<<s<<std::endl;
//-- Display text
glColor3f(1.0, 1.0, 1.0);
glRasterPos2i(10, windowHeight-(camRatio*camImage.size().height)-20);
void * font = GLUT_BITMAP_9_BY_15;
for (std::string::iterator i = s.begin(); i != s.end(); ++i)
{
char c = *i;
glutBitmapCharacter(font, c);
}
}
//-- Display image
glRasterPos2i(0, windowHeight-(camRatio*camImage.size().height));
cv::flip(camImage, camImage, 0);
cv::resize(camImage, camImage, cv::Size(camRatio*camWidth, camRatio*camHeight), 0, 0, cv::INTER_CUBIC);
glDrawPixels( camImage.size().width, camImage.size().height, GL_BGR, GL_UNSIGNED_BYTE, camImage.ptr() );
//-- Load matrix
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
}
static void WaveExtract(TILE &t, cv::Mat &Integral, cv::Mat &IntegtralRotated, cv::Mat &FeatMat)
{
t.status = CInspect::ERR_INSP::OK;
// m_ImgRun already transformed by wavelet xfrm
float* pFeat = (float*)FeatMat.ptr(t.Index);
cv::Mat temp;
// row pointers for integral rectangles
int Sx = t.tilewidth/4;
int Sy = Sx;
// pointers for integral rectangles
double *pTL,*pTML,*pTC,*pTMR,*pTR;
double *pTLM, *pTRM;
double *pLC;
double *pBLM, *pBRM;
double *pBL,*pBML,*pBC,*pBMR,*pBR;
double right,bottom,columb,middle,center;
int w = t.tilewidth;
int h = t.tileheight;
int row = t.row;
int col = t.col;
double TotalBox;
if(WAVEDATASIZE == 10)
{
// take 10 Haar like features from integral images
pTL = ((double*)Integral.ptr(row))+col; pTML = pTL + Sx; pTC = pTL + Sx*2; pTMR = pTL + Sx*3; pTR = pTL + w;
pTLM = ((double*)Integral.ptr(row+Sy))+col; pTRM = pTLM + w;
pLC = ((double*)Integral.ptr(row+Sy*2))+col;
pBLM = ((double*)Integral.ptr(row+Sy*3))+col; pBRM = pBLM + w;
pBL = ((double*)Integral.ptr(row+h))+col; pBML = pTL + Sx; pBC = pBL + Sx*2; pBMR = pBL + Sx*3; pBR = pBL + w;
TotalBox = TOTAL;
right = RIGHT;
bottom = BOTTOM;
columb = COLUMB;
middle = MIDDLE;
center = CENTER;
// left-right
*pFeat++ = (float)(TotalBox - 2 * RIGHT);
// top-bottom
*pFeat++ = (float)(TotalBox - 2 * BOTTOM);
// left+right-middle
*pFeat++ = (float)(TotalBox - 2 * COLUMB);
// top+bottom-middle
*pFeat++ = (float)(TotalBox - 2 * MIDDLE);
// total-center
*pFeat++ = (float)(TotalBox - 4 * CENTER);
// take 10 Haar like features from IntegralRotated images
pTL = ((double*)IntegtralRotated.ptr(row))+col; pTML = pTL + Sx; pTC = pTL + Sx*2; pTMR = pTL + Sx*3; pTR = pTL + w;
pTLM = ((double*)IntegtralRotated.ptr(row+Sy))+col; pTRM = pTLM + w;
pLC = ((double*)IntegtralRotated.ptr(row+Sy*2))+col;
pBLM = ((double*)IntegtralRotated.ptr(row+Sy*3))+col; pBRM = pBLM + w;
pBL = ((double*)IntegtralRotated.ptr(row+h))+col; pBML = pTL + Sx; pBC = pBL + Sx*2; pBMR = pBL + Sx*3; pBR = pBL + w;
TotalBox = TOTAL;
// left-right
*pFeat++ = (float)(TotalBox - 2 * RIGHT);
// top-bottom
*pFeat++ = (float)(TotalBox - 2 * BOTTOM);
// left+right-middle
*pFeat++ = (float)(TotalBox - 2 * COLUMB);
// top+bottom-middle
*pFeat++ = (float)(TotalBox - 2 * MIDDLE);
// total-center
*pFeat++ = (float)(TotalBox - 4 * CENTER);
}
t.datasize = WAVEDATASIZE;
}
// showDebugScores() is a convenience debug function to call if you are having issues....
// This codew WILL BE OUT OF DATE -- Modify to show what you need to view.
static void showDebugScores(TILE &T, KNOWLEDGE &K,cv::Mat &FeatMat)
{
char msg[500];
sprintf_s(msg,500,"Tile Inspect Wavelet (%4d, %4d) = \n",T.col,T.row);
OutputDebugStringA(msg);
float *pF = (float*)FeatMat.ptr(T.Index);
char temp[100];
cv::Mat CvInv = K.covar.InverseMatrix();
if(CvInv.empty())
OutputDebugStringA("Knowledge Empty\n");
else
{
sprintf_s(msg,500,"Tile Knowledge \n");
OutputDebugStringA(msg);
sprintf_s(msg,500,"Tile Feature = \n");
OutputDebugStringA(msg);
sprintf_s(msg,500,"%6.2f",pF[0]);
for( int j=1; j<4; j++)
{
sprintf_s(temp,100,", %6.2f",pF[j]);
strncat_s(msg,500,temp,100);
}
strncat_s(msg,500,"\n",1);
OutputDebugStringA(msg);
pF += 4;
sprintf_s(msg,500,"%6.2f",pF[0]);
for( int j=1; j<3; j++)
{
sprintf_s(temp,100,", %6.2f",pF[j]);
strncat_s(msg,500,temp,100);
}
strncat_s(msg,500,"\n",1);
OutputDebugStringA(msg);
pF += 3;
sprintf_s(msg,500,"%6.2f",pF[0]);
for( int j=1; j<2; j++)
{
sprintf_s(temp,100,", %6.2f",pF[j]);
strncat_s(msg,500,temp,100);
}
strncat_s(msg,500,"\n",1);
OutputDebugStringA(msg);
pF += 2;
sprintf_s(msg,500,"%6.2f\n",pF[0]);
OutputDebugStringA(msg);
for(int i=0; i<CvInv.rows; i++)
{
pF = (float*)CvInv.ptr(i);
sprintf_s(msg,500,"%11.2f",pF[0]);
for( int j=1; j<CvInv.cols; j++)
{
sprintf_s(temp,100,", %11.2f",pF[j]);
strncat_s(msg,500,temp,100);
}
strncat_s(msg,500,"\n",1);
OutputDebugStringA(msg);
}
pF = (float*)CvInv.ptr(0);
if(pF[0] == 0.0F)
pF = (float*)CvInv.ptr(1) + 1;
}
}
bool cnn_master::extract_blob( const string &blob_name, /* in : the name of the blob */
const std::vector<cv::Mat> &input_images, /* in : input img */
cv::Mat &cnn_feature /* out: output cnn feature */
)
{
/* check check check */
if(!is_model_ready())
{
cout<<"error, Model not ready yet"<<endl;
return false;
}
if(input_images.empty())
{
cout<<"error, input_images is empty, return"<<endl;
return false;
}
if( !m_network->has_blob(blob_name))
{
cout<<"error, Net does not have the blob "<<blob_name<<endl;
return false;
}
/* get the infos about every blob */
const std::vector<string> blob_names = m_network->blob_names();
const std::vector<string> layer_names = m_network->layer_names();
/* lock and load */
const shared_ptr<Blob<float> > input_blob = m_network->blob_by_name( blob_names[0] );
/* make sure the image'size is the same with the input layer */
for(unsigned int c=0;c<input_images.size();c++)
{
if( input_images[c].channels() != m_input_channels)
{
cout<<"error, inpur image should have "<<m_input_channels<<" channels, instead of "<<input_images[c].channels()<<endl;
return false;
}
if( input_images[c].cols != m_input_width || input_images[c].rows != m_input_height)
{
cout<<"error, input image should have width "<<m_input_width<<" and height "<<m_input_height<<endl;
return false;
}
}
shared_ptr<caffe::MemoryDataLayer<float> > md_layer = boost::dynamic_pointer_cast <caffe::MemoryDataLayer<float> >(m_network->layers()[0]);
if( !md_layer)
{
cout<<"error, The first layer is not momory data layer"<<endl;
return false;
}
/* prepare the output blob */
const shared_ptr<Blob<float> > output_blob = m_network->blob_by_name( blob_name );
/* prepare the output memory */
int output_dimension = output_blob->count() / output_blob->num();
cnn_feature = cv::Mat::zeros( input_images.size(), output_dimension, CV_32F);
/* once a m_batch_size */
std::vector<cv::Mat>::const_iterator start_iter = input_images.begin();
std::vector<cv::Mat>::const_iterator end_iter = input_images.begin() +
( m_batch_size > input_images.size()? input_images.size():m_batch_size);
while(1)
{
if( start_iter >= input_images.end())
break;
if( end_iter >= input_images.end()) /* adjust the input batch if it is too short */
end_iter = input_images.end();
std::vector<cv::Mat> mat_batch( start_iter, end_iter );
/* extract the features and store */
std::vector<int> no_use_labels( mat_batch.size(), 0 );
md_layer->set_batch_size( mat_batch.size() ); /* sometimes it may less than m_batch_size */
md_layer->AddMatVector( mat_batch, no_use_labels);
/* fire the network */
float no_use_loss=0;
m_network->ForwardPrefilled(&no_use_loss);
/* store the blob to Mat */
const float *feature_blob_data = NULL;
for( unsigned int c=0;c<output_blob->num();c++)
{
feature_blob_data = output_blob->cpu_data() + output_blob->offset(c);
memcpy( cnn_feature.ptr( start_iter - input_images.begin() + c ), feature_blob_data, sizeof(float)*output_dimension);
}
/* update the iterator */
start_iter = end_iter;
if(m_batch_size > input_images.end() - start_iter)
end_iter = input_images.end();
else
end_iter = start_iter + m_batch_size;
}
return true;
}
static void computeLdbDescriptor(const cv::KeyPoint& kpt,
const cv::Mat& img,
const cv::Mat& sum,
unsigned char * desc,
const int& patch_size,
const std::vector<std::vector<int> >& coordinates2by2_,
const std::vector<std::vector<int> >& coordinates3by3_,
const std::vector<std::vector<int> >& coordinates4by4_,
const std::vector<std::vector<int> >& coordinates5by5_,
const std::vector<int>& randSequence,
bool flag)
{
#ifdef TRAINING
for(int i = 0; i < randSequence.size(); i++)
bit_pattern_256_[i] = randSequence[i];
#endif
// Compute the pointer to the center of the feature
int img_y = (int)(kpt.pt.y + 0.5);
int img_x = (int)(kpt.pt.x + 0.5);
const int * center = reinterpret_cast<const int *> (sum.ptr(img_y)) + img_x;
int sum2by2[4], sum3by3[9], sum4by4[16], sum5by5[25];
int dx2by2[4], dx3by3[9], dx4by4[16], dx5by5[25];
int dy2by2[4], dy3by3[9], dy4by4[16], dy5by5[25];
// int dxy2by2[4], dxy3by3[9], dxy4by4[16], dxy5by5[25];
int sum2by2_size = 4, sum3by3_size = 9, sum4by4_size = 16, sum5by5_size = 25;
int offset = patch_size/2;
cv::Mat win_integral_image(patch_size+1, patch_size+1, CV_32S);
if(flag == true)
rotatedIntegralImage(kpt.angle, kpt, img, patch_size, win_integral_image);
for(int i = 0; i < sum2by2_size; i++){
if(flag == false){
int a = coordinates2by2_[i][1] * sum.cols + coordinates2by2_[i][0];
int b = coordinates2by2_[i][3] * sum.cols + coordinates2by2_[i][2];
int c = coordinates2by2_[i][5] * sum.cols + coordinates2by2_[i][4];
int d = coordinates2by2_[i][7] * sum.cols + coordinates2by2_[i][6];
sum2by2[i] = *(center + a) + *(center + d) - *(center + c) - *(center + b);
int e = coordinates2by2_[i][9] * sum.cols + coordinates2by2_[i][8];
int f = coordinates2by2_[i][11] * sum.cols + coordinates2by2_[i][10];
int g = coordinates2by2_[i][13] * sum.cols + coordinates2by2_[i][12];
int h = coordinates2by2_[i][15] * sum.cols + coordinates2by2_[i][14];
dx2by2[i] = sum2by2[i] - 2*(*(center + a) + *(center + g) - *(center + e) - *(center + c));
dy2by2[i] = sum2by2[i] - 2*(*(center + a) + *(center + f) - *(center + b) - *(center + h));
}
else{
sum2by2[i] = win_integral_image.at<int>(coordinates2by2_[i][1]+offset, coordinates2by2_[i][0]+offset)
+ win_integral_image.at<int>(coordinates2by2_[i][7]+offset, coordinates2by2_[i][6]+offset)
- win_integral_image.at<int>(coordinates2by2_[i][3]+offset, coordinates2by2_[i][2]+offset)
- win_integral_image.at<int>(coordinates2by2_[i][5]+offset, coordinates2by2_[i][4]+offset);
dx2by2[i] = sum2by2[i]
- 2*(win_integral_image.at<int>(coordinates2by2_[i][1]+offset, coordinates2by2_[i][0]+offset)
+ win_integral_image.at<int>(coordinates2by2_[i][13]+offset, coordinates2by2_[i][12]+offset)
- win_integral_image.at<int>(coordinates2by2_[i][9]+offset, coordinates2by2_[i][8]+offset)
- win_integral_image.at<int>(coordinates2by2_[i][5]+offset, coordinates2by2_[i][4]+offset));
dy2by2[i] = sum2by2[i]
- 2*(win_integral_image.at<int>(coordinates2by2_[i][1]+offset, coordinates2by2_[i][0]+offset)
+ win_integral_image.at<int>(coordinates2by2_[i][11]+offset, coordinates2by2_[i][10]+offset)
- win_integral_image.at<int>(coordinates2by2_[i][3]+offset, coordinates2by2_[i][2]+offset)
- win_integral_image.at<int>(coordinates2by2_[i][15]+offset, coordinates2by2_[i][14]+offset));
}
}
for(int i = 0; i < sum3by3_size; i++){
if(flag == false){
int a = coordinates3by3_[i][1] * sum.cols + coordinates3by3_[i][0];
int b = coordinates3by3_[i][3] * sum.cols + coordinates3by3_[i][2];
int c = coordinates3by3_[i][5] * sum.cols + coordinates3by3_[i][4];
int d = coordinates3by3_[i][7] * sum.cols + coordinates3by3_[i][6];
sum3by3[i] = *(center + a) + *(center + d) - *(center + c) - *(center + b);
int e = coordinates3by3_[i][9] * sum.cols + coordinates3by3_[i][8];
int f = coordinates3by3_[i][11] * sum.cols + coordinates3by3_[i][10];
int g = coordinates3by3_[i][13] * sum.cols + coordinates3by3_[i][12];
int h = coordinates3by3_[i][15] * sum.cols + coordinates3by3_[i][14];
dx3by3[i] = sum3by3[i] - 2*(*(center + a) + *(center + g) - *(center + e) - *(center + c));
dy3by3[i] = sum3by3[i] - 2*(*(center + a) + *(center + f) - *(center + b) - *(center + h));}
else{
sum3by3[i] = win_integral_image.at<int>(coordinates3by3_[i][1]+offset, coordinates3by3_[i][0]+offset)
+ win_integral_image.at<int>(coordinates3by3_[i][7]+offset, coordinates3by3_[i][6]+offset)
- win_integral_image.at<int>(coordinates3by3_[i][3]+offset, coordinates3by3_[i][2]+offset)
- win_integral_image.at<int>(coordinates3by3_[i][5]+offset, coordinates3by3_[i][4]+offset);
dx3by3[i] = sum3by3[i]
- 2*(win_integral_image.at<int>(coordinates3by3_[i][1]+offset, coordinates3by3_[i][0]+offset)
+ win_integral_image.at<int>(coordinates3by3_[i][13]+offset, coordinates3by3_[i][12]+offset)
- win_integral_image.at<int>(coordinates3by3_[i][9]+offset, coordinates3by3_[i][8]+offset)
- win_integral_image.at<int>(coordinates3by3_[i][5]+offset, coordinates3by3_[i][4]+offset));
dy3by3[i] = sum3by3[i]
- 2*(win_integral_image.at<int>(coordinates3by3_[i][1]+offset, coordinates3by3_[i][0]+offset)
+ win_integral_image.at<int>(coordinates3by3_[i][11]+offset, coordinates3by3_[i][10]+offset)
- win_integral_image.at<int>(coordinates3by3_[i][3]+offset, coordinates3by3_[i][2]+offset)
- win_integral_image.at<int>(coordinates3by3_[i][15]+offset, coordinates3by3_[i][14]+offset));
}
}
for(int i = 0; i < sum4by4_size; i++){
if(flag == false){
int a = coordinates4by4_[i][1] * sum.cols + coordinates4by4_[i][0];
int b = coordinates4by4_[i][3] * sum.cols + coordinates4by4_[i][2];
int c = coordinates4by4_[i][5] * sum.cols + coordinates4by4_[i][4];
int d = coordinates4by4_[i][7] * sum.cols + coordinates4by4_[i][6];
sum4by4[i] = *(center + a) + *(center + d) - *(center + c) - *(center + b);
int e = coordinates4by4_[i][9] * sum.cols + coordinates4by4_[i][8];
int f = coordinates4by4_[i][11] * sum.cols + coordinates4by4_[i][10];
int g = coordinates4by4_[i][13] * sum.cols + coordinates4by4_[i][12];
int h = coordinates4by4_[i][15] * sum.cols + coordinates4by4_[i][14];
dx4by4[i] = sum4by4[i] - 2*(*(center + a) + *(center + g) - *(center + e) - *(center + c));
dy4by4[i] = sum4by4[i] - 2*(*(center + a) + *(center + f) - *(center + b) - *(center + h));
}
else{
sum4by4[i] = win_integral_image.at<int>(coordinates4by4_[i][1]+offset, coordinates4by4_[i][0]+offset)
+ win_integral_image.at<int>(coordinates4by4_[i][7]+offset, coordinates4by4_[i][6]+offset)
- win_integral_image.at<int>(coordinates4by4_[i][3]+offset, coordinates4by4_[i][2]+offset)
- win_integral_image.at<int>(coordinates4by4_[i][5]+offset, coordinates4by4_[i][4]+offset);
dx4by4[i] = sum4by4[i]
- 2*(win_integral_image.at<int>(coordinates4by4_[i][1]+offset, coordinates4by4_[i][0]+offset)
+ win_integral_image.at<int>(coordinates4by4_[i][13]+offset, coordinates4by4_[i][12]+offset)
- win_integral_image.at<int>(coordinates4by4_[i][9]+offset, coordinates4by4_[i][8]+offset)
- win_integral_image.at<int>(coordinates4by4_[i][5]+offset, coordinates4by4_[i][4]+offset));
dy4by4[i] = sum4by4[i]
- 2*(win_integral_image.at<int>(coordinates4by4_[i][1]+offset, coordinates4by4_[i][0]+offset)
+ win_integral_image.at<int>(coordinates4by4_[i][11]+offset, coordinates4by4_[i][10]+offset)
- win_integral_image.at<int>(coordinates4by4_[i][3]+offset, coordinates4by4_[i][2]+offset)
- win_integral_image.at<int>(coordinates4by4_[i][15]+offset, coordinates4by4_[i][14]+offset));
}
}
#ifdef LEVEL5
for(int i = 0; i < sum5by5_size; i++){
if(flag == false){
int a = coordinates5by5_[i][1] * sum.cols + coordinates5by5_[i][0];
int b = coordinates5by5_[i][3] * sum.cols + coordinates5by5_[i][2];
int c = coordinates5by5_[i][5] * sum.cols + coordinates5by5_[i][4];
int d = coordinates5by5_[i][7] * sum.cols + coordinates5by5_[i][6];
sum5by5[i] = *(center + a) + *(center + d) - *(center + c) - *(center + b);
int e = coordinates5by5_[i][9] * sum.cols + coordinates5by5_[i][8];
int f = coordinates5by5_[i][11] * sum.cols + coordinates5by5_[i][10];
int g = coordinates5by5_[i][13] * sum.cols + coordinates5by5_[i][12];
int h = coordinates5by5_[i][15] * sum.cols + coordinates5by5_[i][14];
dx5by5[i] = sum5by5[i] - 2*(*(center + a) + *(center + g) - *(center + e) - *(center + c));
dy5by5[i] = sum5by5[i] - 2*(*(center + a) + *(center + f) - *(center + b) - *(center + h));
}
else{
sum5by5[i] = win_integral_image.at<int>(coordinates5by5_[i][1]+offset, coordinates5by5_[i][0]+offset)
+ win_integral_image.at<int>(coordinates5by5_[i][7]+offset, coordinates5by5_[i][6]+offset)
- win_integral_image.at<int>(coordinates5by5_[i][3]+offset, coordinates5by5_[i][2]+offset)
- win_integral_image.at<int>(coordinates5by5_[i][5]+offset, coordinates5by5_[i][4]+offset);
dx5by5[i] = sum5by5[i]
- 2*(win_integral_image.at<int>(coordinates5by5_[i][1]+offset, coordinates5by5_[i][0]+offset)
+ win_integral_image.at<int>(coordinates5by5_[i][13]+offset, coordinates5by5_[i][12]+offset)
- win_integral_image.at<int>(coordinates5by5_[i][9]+offset, coordinates5by5_[i][8]+offset)
- win_integral_image.at<int>(coordinates5by5_[i][5]+offset, coordinates5by5_[i][4]+offset));
dy5by5[i] = sum5by5[i]
- 2*(win_integral_image.at<int>(coordinates5by5_[i][1]+offset, coordinates5by5_[i][0]+offset)
+ win_integral_image.at<int>(coordinates5by5_[i][11]+offset, coordinates5by5_[i][10]+offset)
- win_integral_image.at<int>(coordinates5by5_[i][3]+offset, coordinates5by5_[i][2]+offset)
- win_integral_image.at<int>(coordinates5by5_[i][15]+offset, coordinates5by5_[i][14]+offset));
}
}
#endif
int pt = 0;
int entire_pt = 0;
uchar desc_bitstring[selected_bits];
static const uchar score[] = {1 << 0, 1 << 1, 1 << 2, 1 << 3, 1 << 4, 1 << 5, 1 << 6, 1 << 7};
for(int i = 0; i < sum2by2_size; i++){
int sum1 = sum2by2[i];
int dx1 = dx2by2[i];
int dy1 = dy2by2[i];
for(int j = i+1; j < sum2by2_size; j++){
if(bit_pattern_256_[pt] == entire_pt){
int sum2 = sum2by2[j];
int idx = pt % 8;
desc_bitstring[pt] = (sum1 > sum2) ? score[idx] : 0;
pt++;
}
entire_pt++;
if(bit_pattern_256_[pt] == entire_pt){
int dx2 = dx2by2[j];
int idx = pt % 8;
desc_bitstring[pt] = (dx1 > dx2) ? score[idx] : 0;
pt++;
}
entire_pt++;
if(bit_pattern_256_[pt] == entire_pt){
int dy2 = dy2by2[j];
int idx = pt % 8;
desc_bitstring[pt] = (dy1 > dy2) ? score[idx] : 0;
pt++;
}
entire_pt++;
}
}
for(int i = 0; i < sum3by3_size; i++){
int sum1 = sum3by3[i];
int dx1 = dx3by3[i];
int dy1 = dy3by3[i];
for(int j = i+1; j < sum3by3_size; j++){
if(bit_pattern_256_[pt] == entire_pt){
int sum2 = sum3by3[j];
int idx = pt % 8;
desc_bitstring[pt] = (sum1 > sum2) ? score[idx] : 0;
pt++;
}
entire_pt++;
if(bit_pattern_256_[pt] == entire_pt){
int dx2 = dx3by3[j];
int idx = pt % 8;
desc_bitstring[pt] = (dx1 > dx2) ? score[idx] : 0;
pt++;
}
entire_pt++;
if(bit_pattern_256_[pt] == entire_pt){
int dy2 = dy3by3[j];
int idx = pt % 8;
desc_bitstring[pt] = (dy1 > dy2) ? score[idx] : 0;
pt++;
}
entire_pt++;
}
}
for(int i = 0; i < sum4by4_size; i++){
int sum1 = sum4by4[i];
int dx1 = dx4by4[i];
int dy1 = dy4by4[i];
for(int j = i+1; j < sum4by4_size; j++){
if(bit_pattern_256_[pt] == entire_pt){
int sum2 = sum4by4[j];
int idx = pt % 8;
desc_bitstring[pt] = (sum1 > sum2) ? score[idx] : 0;
pt++;
}
entire_pt++;
if(bit_pattern_256_[pt] == entire_pt){
int dx2 = dx4by4[j];
int idx = pt % 8;
desc_bitstring[pt] = (dx1 > dx2) ? score[idx] : 0;
pt++;
}
entire_pt++;
if(bit_pattern_256_[pt] == entire_pt){
int dy2 = dy4by4[j];
int idx = pt % 8;
desc_bitstring[pt] = (dy1 > dy2) ? score[idx] : 0;
pt++;
}
entire_pt++;
}
}
#ifdef LEVEL5
for(int i = 0; i < sum5by5_size; i++){
int sum1 = sum5by5[i];
int dx1 = dx5by5[i];
int dy1 = dy5by5[i];
for(int j = i+1; j < sum5by5_size; j++){
if(bit_pattern_256_[pt] == entire_pt){
int sum2 = sum5by5[j];
int idx = pt % 8;
desc_bitstring[pt] = (sum1 > sum2) ? score[idx] : 0;
pt++;
}
entire_pt++;
if(bit_pattern_256_[pt] == entire_pt){
int dx2 = dx5by5[j];
int idx = pt % 8;
desc_bitstring[pt] = (dx1 > dx2) ? score[idx] : 0;
pt++;
}
entire_pt++;
if(bit_pattern_256_[pt] == entire_pt){
int dy2 = dy5by5[j];
int idx = pt % 8;
desc_bitstring[pt] = (dy1 > dy2) ? score[idx] : 0;
pt++;
}
entire_pt++;
}
}
#endif
for (int i = 0, j = 0; i < selected_bits/8; i++, j += 8)
{
desc[i] = (desc_bitstring[j]) | (desc_bitstring[j+1])
| (desc_bitstring[j+2]) | (desc_bitstring[j+3])
| (desc_bitstring[j+4]) | (desc_bitstring[j+5])
| (desc_bitstring[j+6]) | (desc_bitstring[j+7]);
}
}
bool ConvertDepthCvMat16uToYellowCvMat(const cv::Mat & srcDepthMat, cv::Mat & destMat)
{
if ( srcDepthMat.empty() || destMat.empty() ) {
return false;
}
assert ( srcDepthMat.size() == destMat.size() );
assert ( srcDepthMat.type() == CV_16UC1 );
const int nXRes = srcDepthMat.size().width;
const int nYRes = srcDepthMat.size().height;
const int srcRowStep = srcDepthMat.step;
const uchar * srcRowPtr = NULL;
const unsigned short * srcDataPtr = NULL;
float depthHist[OPENNI_MAX_DEPTH];
memset(depthHist, 0, OPENNI_MAX_DEPTH * sizeof(float));
unsigned int pointsNumber = 0;
srcRowPtr = srcDepthMat.data;
for (int y = 0; y < nYRes; ++y, srcRowPtr += srcRowStep)
{
srcDataPtr = (const unsigned short *)(srcRowPtr);
for (int x = 0; x < nXRes; ++x, ++srcDataPtr)
{
unsigned short tmp = *srcDataPtr;
if ( tmp ) {
++depthHist[tmp];
++pointsNumber;
}
}
}
for (int i = 1; i < OPENNI_MAX_DEPTH; ++i) {
depthHist[i] += depthHist[i-1];
}
if (pointsNumber > 0)
{
for (int i = 1; i < OPENNI_MAX_DEPTH; ++i) {
depthHist[i] = (unsigned int)(256 * (1.0f - depthHist[i] / pointsNumber));
}
}
srcRowPtr = srcDepthMat.data;
for (int y = 0; y < nYRes; ++y, srcRowPtr += srcRowStep)
{
srcDataPtr = (const unsigned short *)(srcRowPtr);
uchar * imageptr = destMat.ptr(y);
for (int x = 0; x < nXRes; ++x, ++srcDataPtr, imageptr += destMat.channels())
{
unsigned short tmp = (*srcDataPtr);
if ( tmp )
{
imageptr[0] = 0;
imageptr[1] = depthHist[tmp];
imageptr[2] = depthHist[tmp];
// imageptr[3] = 0xff;
}
else
{
// imageptr[3] = 0;
imageptr[2] = imageptr[1] = imageptr[0] = 0;
}
}
}
return true;
}
void setSource(cv::Mat frame)
{
bind();
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, frame.cols, frame.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, frame.ptr());
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
unbind();
}
cv::Mat Deformation::DeformByMovingLeastSquares(const cv::Mat& inputImg,
const std::vector<int>& originIndex, const std::vector<int>& targetIndex)
{
int imgW = inputImg.cols;
int imgH = inputImg.rows;
cv::Size imgSize(imgW, imgH);
cv::Mat resImg(imgSize, CV_8UC3);
int markNum = originIndex.size() / 2;
std::vector<double> wList(markNum);
std::vector<MagicMath::Vector2> pHatList(markNum);
std::vector<MagicMath::Vector2> qHatList(markNum);
MagicMath::Vector2 pStar, qStar;
std::vector<MagicMath::Vector2> pList(markNum);
for (int mid = 0; mid < markNum; mid++)
{
pList.at(mid) = MagicMath::Vector2(originIndex.at(mid * 2), originIndex.at(mid * 2 + 1));
}
std::vector<MagicMath::Vector2> qList(markNum);
for (int mid = 0; mid < markNum; mid++)
{
qList.at(mid) = MagicMath::Vector2(targetIndex.at(mid * 2), targetIndex.at(mid * 2 + 1));
}
std::vector<std::vector<double> > aMatList(markNum);
std::vector<bool> visitFlag(imgW * imgH, 0);
for (int hid = 0; hid < imgH; hid++)
{
for (int wid = 0; wid < imgW; wid++)
{
MagicMath::Vector2 pos(wid, hid);
//calculate w
bool isMarkVertex = false;
int markedIndex = -1;
double wSum = 0;
for (int mid = 0; mid < markNum; mid++)
{
//double dTemp = (pos - pList.at(mid)).LengthSquared(); //variable
double dTemp = (pos - pList.at(mid)).Length();
//dTemp = pow(dTemp, 1.25);
if (dTemp < 1.0e-15)
{
isMarkVertex = true;
markedIndex = mid;
break;
}
dTemp = pow(dTemp, 1.25);
wList.at(mid) = 1.0 / dTemp;
wSum += wList.at(mid);
}
//
if (isMarkVertex)
{
const unsigned char* pPixel = inputImg.ptr(hid, wid);
int targetH = targetIndex.at(2 * markedIndex + 1);
int targetW = targetIndex.at(2 * markedIndex);
unsigned char* pResPixel = resImg.ptr(targetH, targetW);
pResPixel[0] = pPixel[0];
pResPixel[1] = pPixel[1];
pResPixel[2] = pPixel[2];
visitFlag.at(targetH * imgW + targetW) = 1;
}
else
{
//Calculate pStar qStar
pStar = MagicMath::Vector2(0.0, 0.0);
qStar = MagicMath::Vector2(0.0, 0.0);
for (int mid = 0; mid < markNum; mid++)
{
pStar += (pList.at(mid) * wList.at(mid));
qStar += (qList.at(mid) * wList.at(mid));
}
pStar /= wSum;
qStar /= wSum;
//Calculate pHat qHat
for (int mid = 0; mid < markNum; mid++)
{
pHatList.at(mid) = pList.at(mid) - pStar;
qHatList.at(mid) = qList.at(mid) - qStar;
}
//Calculate A
MagicMath::Vector2 col0 = pos - pStar;
MagicMath::Vector2 col1(col0[1], -col0[0]);
for (int mid = 0; mid < markNum; mid++)
{
std::vector<double> aMat(4);
MagicMath::Vector2 row1(pHatList.at(mid)[1], -pHatList.at(mid)[0]);
aMat.at(0) = pHatList.at(mid) * col0 * wList.at(mid);
aMat.at(1) = pHatList.at(mid) * col1 * wList.at(mid);
aMat.at(2) = row1 * col0 * wList.at(mid);
aMat.at(3) = row1 * col1 * wList.at(mid);
aMatList.at(mid) = aMat;
}
//Calculate fr(v)
MagicMath::Vector2 fVec(0, 0);
for (int mid = 0; mid < markNum; mid++)
{
fVec[0] += (qHatList.at(mid)[0] * aMatList.at(mid).at(0) + qHatList.at(mid)[1] * aMatList.at(mid).at(2));
fVec[1] += (qHatList.at(mid)[0] * aMatList.at(mid).at(1) + qHatList.at(mid)[1] * aMatList.at(mid).at(3));
}
//Calculate target position
fVec.Normalise();
MagicMath::Vector2 targetPos = fVec * ((pos - pStar).Length()) + qStar;
int targetW = targetPos[0];
int targetH = targetPos[1];
if (targetH >= 0 && targetH < imgH && targetW >= 0 && targetW < imgW)
{
const unsigned char* pPixel = inputImg.ptr(hid, wid);
unsigned char* pResPixel = resImg.ptr(targetH, targetW);
pResPixel[0] = pPixel[0];
pResPixel[1] = pPixel[1];
pResPixel[2] = pPixel[2];
visitFlag.at(targetH * imgW + targetW) = 1;
}
}
}
}
std::vector<int> unVisitVecH;
std::vector<int> unVisitVecW;
for (int hid = 0; hid < imgH; hid++)
{
int baseIndex = hid * imgW;
for (int wid = 0; wid < imgW; wid++)
{
if (!visitFlag.at(baseIndex + wid))
{
unVisitVecH.push_back(hid);
unVisitVecW.push_back(wid);
}
}
}
int minAcceptSize = 4;
int fillTime = 1;
while (unVisitVecH.size() > 0)
{
DebugLog << "unVisit number: " << unVisitVecH.size() << std::endl;
std::vector<int> unVisitVecHCopy = unVisitVecH;
std::vector<int> unVisitVecWCopy = unVisitVecW;
unVisitVecH.clear();
unVisitVecW.clear();
int unVisitSize = unVisitVecHCopy.size();
for (int uid = 0; uid < unVisitSize; uid++)
{
MagicMath::Vector3 avgColor(0, 0, 0);
int hid = unVisitVecHCopy.at(uid);
int wid = unVisitVecWCopy.at(uid);
int avgSize = 0;
if ((hid - 1) >= 0 && visitFlag.at((hid - 1) * imgW + wid))
{
unsigned char* pPixel = resImg.ptr(hid - 1, wid);
avgColor[0] += pPixel[0];
avgColor[1] += pPixel[1];
avgColor[2] += pPixel[2];
avgSize++;
}
if ((hid + 1) < imgH && visitFlag.at((hid + 1) * imgW + wid))
{
unsigned char* pPixel = resImg.ptr(hid + 1, wid);
avgColor[0] += pPixel[0];
avgColor[1] += pPixel[1];
avgColor[2] += pPixel[2];
avgSize++;
}
if ((wid - 1) >= 0 && visitFlag.at(hid * imgW + wid - 1))
{
unsigned char* pPixel = resImg.ptr(hid, wid - 1);
avgColor[0] += pPixel[0];
avgColor[1] += pPixel[1];
avgColor[2] += pPixel[2];
avgSize++;
}
if ((wid + 1) < imgW && visitFlag.at(hid * imgW + wid + 1))
{
unsigned char* pPixel = resImg.ptr(hid, wid + 1);
avgColor[0] += pPixel[0];
avgColor[1] += pPixel[1];
avgColor[2] += pPixel[2];
avgSize++;
}
if (avgSize >= minAcceptSize)
{
visitFlag.at(hid * imgW + wid) = 1;
avgColor /= avgSize;
unsigned char* pFillPixel = resImg.ptr(hid, wid);
pFillPixel[0] = avgColor[0];
pFillPixel[1] = avgColor[1];
pFillPixel[2] = avgColor[2];
}
else
{
unVisitVecH.push_back(hid);
unVisitVecW.push_back(wid);
}
}
if (fillTime == 4)
{
minAcceptSize--;
}
else if (fillTime == 6)
{
minAcceptSize--;
}
else if (fillTime == 8)
{
minAcceptSize--;
}
fillTime++;
}
//fill hole
/*for (int hid = 0; hid < imgH; hid++)
{
int baseIndex = hid * imgW;
for (int wid = 0; wid < imgW; wid++)
{
if (!visitFlag.at(baseIndex + wid))
{
double wSum = 0;
MagicMath::Vector3 avgColor(0, 0, 0);
for (int wRight = wid + 1; wRight < imgW; wRight++)
{
if (visitFlag.at(baseIndex + wRight))
{
double wTemp = 1.0 / (wRight - wid);
wSum += wTemp;
unsigned char* pPixel = resImg.ptr(hid, wRight);
avgColor[0] += wTemp * pPixel[0];
avgColor[1] += wTemp * pPixel[1];
avgColor[2] += wTemp * pPixel[2];
break;
}
}
for (int wLeft = wid - 1; wLeft >= 0; wLeft--)
{
if (visitFlag.at(baseIndex + wLeft))
{
double wTemp = 1.0 / (wid - wLeft);
wSum += wTemp;
unsigned char* pPixel = resImg.ptr(hid, wLeft);
avgColor[0] += wTemp * pPixel[0];
avgColor[1] += wTemp * pPixel[1];
avgColor[2] += wTemp * pPixel[2];
break;
}
}
for (int hUp = hid - 1; hUp >= 0; hUp--)
{
if (visitFlag.at(hUp * imgW + wid))
{
double wTemp = 1.0 / (hid - hUp);
unsigned char* pPixel = resImg.ptr(hUp, wid);
wSum += wTemp;
avgColor[0] += wTemp * pPixel[0];
avgColor[1] += wTemp * pPixel[1];
avgColor[2] += wTemp * pPixel[2];
break;
}
}
for (int hDown = hid + 1; hDown < imgH; hDown++)
{
if (visitFlag.at(hDown * imgW + wid))
{
double wTemp = 1.0 / (hDown - hid);
unsigned char* pPixel = resImg.ptr(hDown, wid);
wSum += wTemp;
avgColor[0] += wTemp * pPixel[0];
avgColor[1] += wTemp * pPixel[1];
avgColor[2] += wTemp * pPixel[2];
break;
}
}
if (wSum > 1.0e-15)
{
avgColor /= wSum;
}
unsigned char* pFillPixel = resImg.ptr(hid, wid);
pFillPixel[0] = avgColor[0];
pFillPixel[1] = avgColor[1];
pFillPixel[2] = avgColor[2];
}
}
}*/
return resImg;
}
unsigned long ipa_Utils::FilterTearOffEdges(cv::Mat& xyzImage, cv::Mat* mask, float piHalfFraction)
{
/// Check if destination image has been initialized correctly
CV_Assert(xyzImage.type() == CV_32FC3);
float pi = 3.14159f;
float t_lower =pi/piHalfFraction;
float t_upper = pi - t_lower;
if(mask)
{
mask->create(xyzImage.size() , CV_8UC3);
mask->setTo(0);
}
for(int row=0; row < xyzImage.rows; row++)
{
int index_vLeft = -1;
int index_vMiddle = -1;
int index_vRight = -1;
int index_vUp = -1;
int index_vDown = -1;
cv::Vec3f vLeft = cv::Vec3f::all(0);
cv::Vec3f vMiddle = cv::Vec3f::all(0);
cv::Vec3f vRight = cv::Vec3f::all(0);
cv::Vec3f vUp = cv::Vec3f::all(0);
cv::Vec3f vDown = cv::Vec3f::all(0);
cv::Vec3f vDiff = cv::Vec3f::all(0);
float* f_image_ptr_RowUp = 0;
float* f_image_ptr_RowMiddle = 0;
float* f_image_ptr_RowDown = 0;
float dot = -1.f;
float angle = -1.f;
if (row-1 >= 0)
{
f_image_ptr_RowUp = xyzImage.ptr<float>(row-1);
}
f_image_ptr_RowMiddle = xyzImage.ptr<float>(row);
if (row+1 < xyzImage.rows)
{
f_image_ptr_RowDown = xyzImage.ptr<float>(row+1);
}
/// Extract four surrounding neighbor vectors that have a non zero mask value
///
/// x
/// x o x
/// x
///
for(int col=0; col < xyzImage.cols; col++)
{
/// Counte the amount of times, we satisfy the thresholds
int score = 0;
/// Vector Middle (must exist)
index_vMiddle = col;
vMiddle[0] = f_image_ptr_RowMiddle[3*index_vMiddle];
vMiddle[1] = f_image_ptr_RowMiddle[3*index_vMiddle + 1];
vMiddle[2] = f_image_ptr_RowMiddle[3*index_vMiddle + 2];
/// Vector Left
if (col-1 >= 0)
{
index_vLeft = col-1;
vLeft[0] = f_image_ptr_RowMiddle[3*index_vLeft];
vLeft[1] = f_image_ptr_RowMiddle[3*index_vLeft + 1];
vLeft[2] = f_image_ptr_RowMiddle[3*index_vLeft + 2];
vDiff = vLeft - vMiddle;
float vLeftNorm = std::sqrt((vLeft[0] * vLeft[0]) +
(vLeft[1] * vLeft[1]) + (vLeft[2] * vLeft[2]));
vLeft[0] = vLeft[0]/vLeftNorm;
vLeft[1] = vLeft[1]/vLeftNorm;
vLeft[2] = vLeft[2]/vLeftNorm;
//vLeft.Normalize();
float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
(vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
vDiff[0] = vDiff[0]/vDiffNorm;
vDiff[1] = vDiff[1]/vDiffNorm;
vDiff[2] = vDiff[2]/vDiffNorm;
//vDiff.Normalize();
dot = (float)vDiff.ddot(vLeft);
//dot = vDiff.Dot(vLeft);
angle = (float)std::acos(dot);
//angle = Wm4::Math<float>::ACos( dot );
if (angle > t_upper || angle < t_lower)
{
score++;
}
else
{
score--;
}
}
/// Vector Right
if (col+1 < xyzImage.rows)
{
index_vRight = col+1;
vRight[0] = f_image_ptr_RowMiddle[3*index_vRight];
vRight[1] = f_image_ptr_RowMiddle[3*index_vRight + 1];
vRight[2] = f_image_ptr_RowMiddle[3*index_vRight + 2];
vDiff = vRight - vMiddle;
float vRightNorm = std::sqrt((vRight[0] * vRight[0]) +
(vRight[1] * vRight[1]) + (vRight[2] * vRight[2]));
vRight[0] = vRight[0]/vRightNorm;
vRight[1] = vRight[1]/vRightNorm;
vRight[2] = vRight[2]/vRightNorm;
//vRight.Normalize();
float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
(vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
vDiff[0] = vDiff[0]/vDiffNorm;
vDiff[1] = vDiff[1]/vDiffNorm;
vDiff[2] = vDiff[2]/vDiffNorm;
//vDiff.Normalize();
dot = (float)vDiff.ddot(vLeft);
//dot = vDiff.Dot(vLeft);
angle = (float)std::acos(dot);
//angle = Wm4::Math<float>::ACos( dot );
if (angle > t_upper || angle < t_lower)
{
score++;
}
else
{
score--;
}
}
/// Vector Up
if (f_image_ptr_RowUp)
{
index_vUp = col;
vUp[0] = f_image_ptr_RowUp[3*index_vUp];
vUp[1] = f_image_ptr_RowUp[3*index_vUp + 1];
vUp[2] = f_image_ptr_RowUp[3*index_vUp + 2];
vDiff = vUp - vMiddle;
float vUpNorm = std::sqrt((vUp[0] * vUp[0]) +
(vUp[1] * vUp[1]) + (vUp[2] * vUp[2]));
vUp[0] = vUp[0]/vUpNorm;
vUp[1] = vUp[1]/vUpNorm;
vUp[2] = vUp[2]/vUpNorm;
//vUp.Normalize();
float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
(vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
vDiff[0] = vDiff[0]/vDiffNorm;
vDiff[1] = vDiff[1]/vDiffNorm;
vDiff[2] = vDiff[2]/vDiffNorm;
//vDiff.Normalize();
dot = (float)vDiff.ddot(vLeft);
//dot = vDiff.Dot(vLeft);
angle = (float)std::acos(dot);
//angle = Wm4::Math<float>::ACos( dot );
if (angle > t_upper || angle < t_lower)
{
score++;
}
else
{
score--;
}
}
/// Vector Down
if (f_image_ptr_RowDown)
{
index_vDown = col;
vDown[0] = f_image_ptr_RowDown[3*index_vDown];
vDown[1] = f_image_ptr_RowDown[3*index_vDown + 1];
vDown[2] = f_image_ptr_RowDown[3*index_vDown + 2];
float vDownNorm = std::sqrt((vDown[0] * vDown[0]) +
(vDown[1] * vDown[1]) + (vDown[2] * vDown[2]));
vDown[0] = vDown[0]/vDownNorm;
vDown[1] = vDown[1]/vDownNorm;
vDown[2] = vDown[2]/vDownNorm;
//vDown.Normalize();
float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
(vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
vDiff[0] = vDiff[0]/vDiffNorm;
vDiff[1] = vDiff[1]/vDiffNorm;
vDiff[2] = vDiff[2]/vDiffNorm;
//vDiff.Normalize();
dot = (float)vDiff.ddot(vLeft);
//dot = vDiff.Dot(vLeft);
angle = (float)std::acos(dot);
//angle = Wm4::Math<float>::ACos( dot );
if (angle > t_upper || angle < t_lower)
{
score++;
}
else
{
score--;
}
}
/// Mask value if angle exceeded threshold too often
if (score > 0)
{
cv::Vec3b pt(0, 0, 0);
if(mask)
{
mask->at<cv::Vec3b>(row,col)=pt;
}
// xyzImage.at<cv::Vec3f>(row, col) = pt;
for(int i = 0; i < 3; i++)
((float*)xyzImage.ptr(row))[3*col+i] = 0.f;
}
}
}
return ipa_Utils::RET_OK;
}
/****************************************************************************
**
** SaveAsPCX
**
** Write a PCX file version of the image
*/
int CPalette::SavePalettizedAsPCX( cv::Mat& image, // matrix is Palettized pixels
char* pcxfile
)
{
int stat = 0;
double nrgbr = 63, nrgbg =63, nrgbb = 63;
int resx = image.cols;
int resy = image.rows;
int npal = m_PaletteSize;
ColorData *colortable = m_table;
// COctreeNode *octree = m_pOctree;
PCXhdr hdr;
// unsigned char *buf;
PCXCOLOR *pal;
int i,k;
// RGBType color;
unsigned char *pPix;
FILE *pf;
fopen_s(&pf,pcxfile, "wb");
if (pf == NULL)
{
// fprintf(stderr, "Can't open file %s for writing\n", pcxfile);
return 1;
}
/*
** Set up the PCX Palettte.
*/
pal = (PCXCOLOR *)malloc(256*sizeof(PCXCOLOR));
if(!pal)
{
stat = 2;
goto ESCAPE2;
}
for (k = 0; k < npal ; k++)
{
pal[k].r = (unsigned char)(colortable[k].color.r);
pal[k].g = (unsigned char)(colortable[k].color.g);
pal[k].b = (unsigned char)(colortable[k].color.b);
}
/*
** Fill in the PCX header
*/
hdr.manuf = (char)10;
hdr.vers = (char)5;
hdr.rle = (char)1;
hdr.bitpx = 8;
hdr.x1 = hdr.y1 = 0;
hdr.x2 = resx - 1;
hdr.y2 = resy - 1;
hdr.hres = 640;
hdr.vres = 480;
hdr.hscreen = 0;
hdr.vscreen = 0;
hdr.bpline = hdr.x2;
hdr.vmode = (char)0;
hdr.nplanes = 1;
hdr.palinfo = 1;
memset(hdr.xtra,0x00,54);
fwrite(&hdr, sizeof(PCXhdr), 1, pf);
/*
** Scan the image and pack pixels for the PCX image
*/
for (i = 0; i < resy; i++)
{
pPix = image.ptr(i);
/*
** Encode this scan line and write it to the file. If return is non-0
** then the write failed
*/
if (encline(pPix, hdr.x2, pf))
{
stat = 3;
goto ESCAPE1;
}
}
/*
** Store the palette
*/
#if(0)
for (i = 0; i < 256; i++)
{
pal[i].r *= 4;
pal[i].g *= 4;
pal[i].b *= 4;
}
#endif
putc(0x0c, pf);
fwrite(pal, 768, 1, pf);
ESCAPE1:
free(pal);
ESCAPE2:
fclose(pf);
return stat;
}
void NormalizeResponse(
const cv::Mat& src,
cv::Mat& dst,
int winsize,
int ftzero,
uchar* buf)
{
if (dst.empty())
{
dst.create(src.size(), CV_8U);
}
// Source code from OpenCV: modules/calib3d/src/stereobm.cpp
int x, y, wsz2 = winsize / 2;
int* vsum = reinterpret_cast<int*>(cv::alignPtr(buf + (wsz2 + 1) * sizeof(vsum[0]), 32));
int scale_g = winsize * winsize / 8, scale_s = (1024 + scale_g) / (scale_g * 2);
const int OFS = 256 * 5, TABSZ = OFS * 2 + 256;
uchar tab[TABSZ];
const uchar* sptr = src.ptr();
int srcstep = src.step;
cv::Size size = src.size();
scale_g *= scale_s;
for (x = 0; x < TABSZ; x++)
{
tab[x] = (uchar)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero * 2 : x - OFS + ftzero);
}
for (x = 0; x < size.width; x++)
{
vsum[x] = (ushort)(sptr[x] * (wsz2 + 2));
}
for (y = 1; y < wsz2; y++)
{
for (x = 0; x < size.width; x++)
{
vsum[x] = (ushort)(vsum[x] + sptr[srcstep*y + x]);
}
}
for (y = 0; y < size.height; y++)
{
const uchar* top = sptr + srcstep * MAX(y - wsz2 - 1, 0);
const uchar* bottom = sptr + srcstep * MIN(y + wsz2, size.height - 1);
const uchar* prev = sptr + srcstep * MAX(y - 1, 0);
const uchar* curr = sptr + srcstep * y;
const uchar* next = sptr + srcstep * MIN(y + 1, size.height - 1);
uchar* dptr = dst.ptr<uchar>(y);
for (x = 0; x < size.width; x++)
{
vsum[x] = (ushort)(vsum[x] + bottom[x] - top[x]);
}
for (x = 0; x <= wsz2; x++)
{
vsum[-x-1] = vsum[0];
vsum[size.width + x] = vsum[size.width - 1];
}
int sum = vsum[0] * (wsz2 + 1);
for( x = 1; x <= wsz2; x++ )
{
sum += vsum[x];
}
int val = ((curr[0] * 5 + curr[1] + prev[0] + next[0]) * scale_g - sum * scale_s) >> 10;
dptr[0] = tab[val + OFS];
for( x = 1; x < size.width-1; x++ )
{
sum += vsum[x + wsz2] - vsum[x - wsz2 - 1];
val = ((curr[x] * 4 + curr[x - 1] + curr[x + 1] + prev[x] + next[x]) * scale_g - sum * scale_s) >> 10;
dptr[x] = tab[val + OFS];
}
sum += vsum[x+wsz2] - vsum[x - wsz2 - 1];
val = ((curr[x] * 5 + curr[x - 1] + prev[x] + next[x]) * scale_g - sum * scale_s) >> 10;
dptr[x] = tab[val + OFS];
}
}
void PPTestWindow::showResultMap(const cv::Mat& aImage, unsigned int aProgress, unsigned int aSolutionID, double aTotalDist) {
result_tabs_views.push_back(new Ui::PPResultView);
result_tabs_widgets.push_back(new QWidget(tabWidget));
{ std::stringstream ss;
ss << "Result " << aSolutionID;
tabWidget->addTab(result_tabs_widgets.back(), QString::fromStdString(ss.str()));
result_tabs_views.back()->setupUi(result_tabs_widgets.back());
ss << " with " << aProgress << " vertices, achieves a total traveled distance of " << aTotalDist;
result_tabs_views.back()->label->setText(QString::fromStdString(ss.str())); };
result_tabs_views.back()->canvas->setScene(new QGraphicsScene(result_tabs_views.back()->canvas));
result_tabs_pixmaps.push_back(result_tabs_views.back()->canvas->scene()->addPixmap(QPixmap::fromImage(QImage(aImage.ptr(),
aImage.size().width,
aImage.size().height,
QImage::Format_RGB888
).rgbSwapped()
)
)
);
{ std::stringstream ss;
ss << "rrt_result_v" << aProgress << "_s" << aSolutionID << "_d" << aTotalDist << ".png";
result_names.push_back(ss.str()); };
};
void GaborFilter::filter(const cv::Mat& grayFrame, std::vector<cv::Mat>& projections,
const cv::Mat& mask, int neighborhood, bool normalize) const {
cv::Mat dilatedMask;
if (!mask.empty() && neighborhood > 1) {
cv::dilate(mask, dilatedMask, cv::Mat(neighborhood, neighborhood, CV_8U, cv::Scalar(255)));
}
else {
dilatedMask = mask;
}
int dimensions = kernels.size();
projections.resize(dimensions);
for (int k = 0; k < dimensions; ++k) {
projections[k].create(grayFrame.size(), CV_32F);
projections[k].setTo(cv::Scalar(0));
for (int y = 0; y < grayFrame.rows; ++y) {
const uchar* maskPtr = (!dilatedMask.empty() ? dilatedMask.ptr(y) : NULL);
float* projPtr = projections[k].ptr<float> (y);
for (int x = 0; x < grayFrame.cols; ++x) {
float val = 0;
if (!maskPtr || maskPtr[x] > 0) {
for (int dy = -kernelRadius; dy <= kernelRadius; ++dy) {
int fy = y + dy;
if (fy >= 0 && fy < grayFrame.rows) {
const uchar* fPtr = grayFrame.ptr(fy);
const float* kPtr = kernels[k].ptr<float> (dy + kernelRadius);
for (int dx = -kernelRadius; dx <= kernelRadius; ++dx) {
int fx = x + dx;
if (fx >= 0 && fx < grayFrame.cols) {
val += fPtr[fx] * kPtr[dx + kernelRadius];
}
}
}
}
}
projPtr[x] = val;
}
}
if (neighborhood > 1) {
cv::Mat integral;
getIntegral(projections[k], integral);
for (int y = 0; y < grayFrame.rows; ++y) {
const uchar* maskPtr = (!mask.empty() ? mask.ptr(y) : NULL);
float* projPtr = projections[k].ptr<float> (y);
int y1 = std::max(y - (neighborhood - 1) / 2, 0);
int y2 = std::min(y + neighborhood / 2, grayFrame.rows - 1);
float* integralPtr1 = (y1 > 0 ? integral.ptr<float> (y1) : NULL);
float* integralPtr2 = integral.ptr<float> (y2);
for (int x = 0; x < grayFrame.cols; ++x) {
float val = 0;
if (!maskPtr || maskPtr[x] > 0) {
int x1 = std::max(x - (neighborhood - 1) / 2, 0);
int x2 = std::min(x + neighborhood / 2, grayFrame.cols - 1);
val += integralPtr2[x2];
if (integralPtr1) {
val -= integralPtr1[x2];
}
if (x1 > 0) {
val -= integralPtr2[x1 - 1];
}
if (integralPtr1 && x1 > 0) {
val += integralPtr1[x1 - 1];
}
val /= ((x2 - x1) + 1) * ((y2 - y1) + 1);
}
projPtr[x] = val;
}
}
}
if (normalize) {
cv::normalize(projections[k], projections[k], 0, 1, cv::NORM_MINMAX, -1, mask);
}
}
}
//
//Write an image to the stream
void ImageSequenceIO::WriteImageToStream(const cv::Mat &image, const int frameId)
{
m_pState->m_ofs.write((char*)&m_pState->m_readFrameId,sizeof(int));
m_pState->m_ofs.write((const char*)image.ptr(),m_pState->m_writeHeader.totalSize());
}
static inline void ippiGetImage(const cv::Mat &src, ::ipp::IwiImage &dst)
{
::ipp::IwiBorderSize inMemBorder;
if(src.isSubmatrix()) // already have physical border
{
cv::Size origSize;
cv::Point offset;
src.locateROI(origSize, offset);
inMemBorder.left = (IwSize)offset.x;
inMemBorder.top = (IwSize)offset.y;
inMemBorder.right = (IwSize)(origSize.width - src.cols - offset.x);
inMemBorder.bottom = (IwSize)(origSize.height - src.rows - offset.y);
}
dst.Init(ippiSize(src.size()), ippiGetDataType(src.depth()), src.channels(), inMemBorder, (void*)src.ptr(), src.step);
}