Ejemplo n.º 1
0
// The image is seperated into two parts given the location of seperation axis x, each part's length maximum loc+1;
// Return the dissimilarity score
float ReidDescriptor::dissym_div(int x, cv::Mat img, cv::Mat MSK, int loc, float alpha)
{
	int H = img.rows;
	int W = img.cols;
	int chs = img.channels();

	cv::Mat imgUP = img.rowRange(0, x + 1); // [0,x]
	cv::Mat imgDOWN = img.rowRange(x, img.rows);
	cv::Mat MSK_U = MSK.rowRange(0, x + 1);
	cv::Mat MSK_D = MSK.rowRange(x, MSK.rows);

	int dimLoc = min(min(x + 1, MSK_D.rows), loc + 1);

	if (dimLoc != 0)
	{
		cv::Mat imgUPloc = img.rowRange(x - dimLoc + 1, x + 1); // [x-dimLoc+1,x]
		cv::Mat imgDWloc;
		cv::flip(imgDOWN.rowRange(0, dimLoc), imgDWloc, 0);
		cv::Mat temp;
		cv::pow(imgUPloc - imgDWloc, 2, temp);
		float ans = alpha * (1 - sqrt(sum(temp.reshape(1))[0]) / dimLoc) +
					(1 - alpha) * (abs(sum(MSK_U)[0] - sum(MSK_D)[0])) / max(MSK_U.rows * MSK_U.cols, MSK_D.rows * MSK_D.cols);

		return ans;
	}
	else
	{
		return 1;
	}
}
	void data_transformer_util::rotate_band(
		cv::Mat image,
		int shift_x_to_left,
		int shift_y_to_top)
	{
		int actual_shift_x = (shift_x_to_left % image.cols);
		if (actual_shift_x < 0)
			actual_shift_x += image.cols;
		int actual_shift_y = (shift_y_to_top % image.rows);
		if (actual_shift_y < 0)
			actual_shift_y += image.rows;
		if ((actual_shift_x == 0) && (actual_shift_y == 0))
			return;

		cv::Mat cloned_image = image.clone();

		if (actual_shift_y == 0)
		{
			cloned_image.colRange(actual_shift_x, image.cols).copyTo(image.colRange(0, image.cols - actual_shift_x));
			cloned_image.colRange(0, actual_shift_x).copyTo(image.colRange(image.cols - actual_shift_x, image.cols));
		}
		else if (actual_shift_x == 0)
		{
			cloned_image.rowRange(actual_shift_y, image.rows).copyTo(image.rowRange(0, image.rows - actual_shift_y));
			cloned_image.rowRange(0, actual_shift_y).copyTo(image.rowRange(image.rows - actual_shift_y, image.rows));
		}
		else
		{
			cloned_image.colRange(actual_shift_x, image.cols).rowRange(actual_shift_y, image.rows).copyTo(image.colRange(0, image.cols - actual_shift_x).rowRange(0, image.rows - actual_shift_y));
			cloned_image.colRange(0, actual_shift_x).rowRange(actual_shift_y, image.rows).copyTo(image.colRange(image.cols - actual_shift_x, image.cols).rowRange(0, image.rows - actual_shift_y));
			cloned_image.colRange(actual_shift_x, image.cols).rowRange(0, actual_shift_y).copyTo(image.colRange(0, image.cols - actual_shift_x).rowRange(image.rows - actual_shift_y, image.rows));
			cloned_image.colRange(0, actual_shift_x).rowRange(0, actual_shift_y).copyTo(image.colRange(image.cols - actual_shift_x, image.cols).rowRange(image.rows - actual_shift_y, image.rows));
		}
	}
Ejemplo n.º 3
0
cv::Mat ReidDescriptor::getWhsvFeature(cv::Mat img, cv::Mat MSK)
{
	int offset = img.rows / 5;
	vector<cv::Mat> sub(5);

	// Divide the image into 5x1 cells
	for(int i = 0 ; i < 4 ; i++) {
		sub[i] = img.rowRange(offset * i, offset * (i + 1));
	}
	sub[4] = img.rowRange(offset * 4, img.rows);
	// Debug this

	cv::Mat conc;
	cv::Mat temp;
	for(int i = 0 ; i < 5 ; i++) {
		cv::Mat HSV = HSVVector(sub[i]);
		if(i == 0) {
			conc = HSV;
		} else {
			vconcat(conc, HSV, conc);
		}
	}

	return conc;
    //return cv::Mat::zeros(2,2,CV_8U);
}
Ejemplo n.º 4
0
void Mat::_removeRowsNonContinuous(cv::Mat &m, 
  const std::vector<unsigned int> &rows)
{
  // always preserve the order of the rest of rows
  
  // remove rows in descending order, grouping when possible
  int end_row = m.rows;
  int i_idx = (int)rows.size() - 1;
  while(i_idx >= 0)
  {
    int j_idx = i_idx - 1;
    while(j_idx >= 0 && ((int)(rows[i_idx] - rows[j_idx]) == i_idx - j_idx))
    {
      j_idx--;
    }
    //data.erase(data.begin() + indices[j_idx + 1], 
    //  data.begin() + indices[i_idx] + 1);
    // ==
    //std::copy( m.ptr<T>(rows[i_idx]+1), m.ptr<T>(end_row),
    //  m.ptr<T>(rows[j_idx + 1]) );
        
    m.rowRange(rows[j_idx+1], rows[j_idx+1] + end_row-rows[i_idx]-1) = 
      m.rowRange(rows[i_idx]+1, end_row) * 1;
    
    end_row -= rows[i_idx] - rows[j_idx+1] + 1;
    
    i_idx = j_idx;
  }
  
  // remove last rows
  m.resize(end_row);
}
Ejemplo n.º 5
0
// Given map kernel calculate the histogram of each part of the image and combine them together.
cv::Mat ReidDescriptor::Whsv_estimation(cv::Mat img, vector<int> NBINs, cv::Mat MAP_KRNL, int HDanti, int TLanti)
{
	cv::Mat img_hsv = img.clone();
	//Matlab:0:1,0:1,0:1 ; Opencv:0:255,0:255,0:180
	cvtColor(img, img_hsv, CV_BGR2HSV);
	vector<cv::Mat>img_split;
	split(img_hsv, img_split);
	img_split[2].convertTo(img_split[2], CV_8UC1, 255);
	equalizeHist(img_split[2], img_split[2]);
	img_split[0].convertTo(img_split[0], CV_32FC1, 1.0 / 180);
	img_split[1].convertTo(img_split[1], CV_32FC1, 1.0 / 255);
	img_split[2].convertTo(img_split[2], CV_32FC1, 1.0 / 255);
	merge(img_split, img_hsv);

	cv::Mat UP = img_hsv.rowRange(HDanti + 1, TLanti + 1);
	vector<cv::Mat> UP_split; split(UP, UP_split);
	cv::Mat UPW = MAP_KRNL.rowRange(HDanti + 1, TLanti + 1);

	cv::Mat DOWN = img_hsv.rowRange(TLanti + 1, img_hsv.rows);
	vector<cv::Mat> DOWN_split; split(DOWN, DOWN_split);
	cv::Mat DOWNW = MAP_KRNL.rowRange(TLanti + 1, img_hsv.rows);;

	UPW = UPW.reshape(1, 1);
	DOWNW = DOWNW.reshape(1, 1);

	cv::Mat tmph0(0, 0, CV_32FC1); cv::Mat tmph2(0, 0, CV_32FC1);
	cv::Mat tmpup0(0, 0, CV_32FC1); cv::Mat tmpup2(0, 0, CV_32FC1);
	cv::Mat tmpdown0(0, 0, CV_32FC1); cv::Mat tmpdown2(0, 0, CV_32FC1);

	for (int ch = 0 ; ch < 3 ; ch++){
		tmph2.push_back(cv::Mat(cv::Mat::zeros(NBINs[ch], 1, CV_32F)));
		cv::Mat rasterUP = UP_split[ch];
		tmpup2.push_back(whistcY(rasterUP.reshape(1, 1), UPW, NBINs[ch]));

		cv::Mat rasterDOWN = DOWN_split[ch];
		tmpdown2.push_back(whistcY(rasterDOWN.reshape(1, 1), DOWNW, NBINs[ch]));
	}

	cv::Mat ans = tmph2;
	ans.push_back(tmpup2);
	ans.push_back(tmpdown2);

	for (int row = 0; row < ans.rows; row++) {
		for (int col = 0; col < ans.cols; col++) {
#ifdef linux
			if (isnan(ans.at<float>(row, col)) == true) {
				ans.at<float>(row, col) = 0;
			}
#endif
#ifdef _WIN32
			if (_isnan(ans.at<float>(row, col)) != 0) {
				ans.at<float>(row, col) = 0;
			}
#endif
		}
	}

	return ans;
}
Ejemplo n.º 6
0
// AVERAGE: 16.46 fps after 600 frames
void extractVU_method2(const cv::Mat &srcNV21, cv::Mat &imageYUV){

    Mat Y, U, V;
    
    size_t height = 2*srcNV21.rows/3;
    
    // Luma
        Y     = srcNV21.rowRange( 0, height);
    
    // Chroma U
    if (U.empty())
    	U.create(cv::Size(srcNV21.cols/2, height/2), CV_8UC1);
    
    // Chroma V
    if (V.empty())
    	V.create(cv::Size(srcNV21.cols/2, height/2), CV_8UC1);   	    
    
    Mat image = srcNV21.rowRange( height, srcNV21.rows);
	size_t nRows = image.rows;   // number of lines
    size_t nCols = image.cols;   // number of columns  

    /// Convert to 1D array if Continuous
    if (image.isContinuous()) {
        nCols = nCols * nRows;
		nRows = 1; // it is now a 
	}   

    for (int j=0; j<nRows; j++) {
    
        /// Pointer to start of the row      
        uchar* data   = reinterpret_cast<uchar*>(image.data);
        uchar* colorV = reinterpret_cast<uchar*>(V.data);
        uchar* colorU = reinterpret_cast<uchar*>(U.data);

		for (int i = 0; i < nCols; i += 2) {
		        // assign each pixel to V and U
                *colorV++ = *data++; //  [0,255]
                *colorU++ = *data++; //  [0,255]   
        }
    }
    
    std::vector<cv::Mat> channels(4);
    
    cv::Mat Yscaled;
    
    cv::resize(Y, Yscaled, cv::Size(srcNV21.cols/2, height/2));    

    channels[0] = Yscaled;
    channels[1] = U;
    channels[2] = V;
    channels[3] = Mat::zeros(cv::Size(srcNV21.cols/2, height/2), CV_8UC1) + 255;

    cv::merge(channels, imageYUV);
}
Ejemplo n.º 7
0
void circshift(cv::Mat &A, int shitf_row, int shift_col) {
    int row = A.rows, col = A.cols;
    shitf_row = (row + (shitf_row % row)) % row;
    shift_col = (col + (shift_col % col)) % col;
    cv::Mat temp = A.clone();
    if (shitf_row){
        temp.rowRange(row - shitf_row, row).copyTo(A.rowRange(0, shitf_row));
        temp.rowRange(0, row - shitf_row).copyTo(A.rowRange(shitf_row, row));
    }
    if (shift_col){
        temp.colRange(col - shift_col, col).copyTo(A.colRange(0, shift_col));
        temp.colRange(0, col - shift_col).copyTo(A.colRange(shift_col, col));
    }
    return;
}
bool VisualFeatureExtraction::cutFeatures(cv::vector<cv::KeyPoint> &kpts,
		cv::Mat &features, unsigned short maxFeats) const {

	// store hash values in a map
	std::map<size_t, unsigned int> keyp_hashes;
	cv::vector<cv::KeyPoint>::iterator itKeyp;

	cv::Mat sorted_features;

	unsigned int iLine = 0;
	for (itKeyp = kpts.begin(); itKeyp < kpts.end(); itKeyp++, iLine++)
		keyp_hashes[(*itKeyp).hash()] = iLine;

	// sort values according to the response
	std::sort(kpts.begin(), kpts.end(), greater_than_response());
	// create a new descriptor matrix with the sorted keypoints
	sorted_features.create(0, features.cols, features.type());
	sorted_features.reserve(features.rows);
	for (itKeyp = kpts.begin(); itKeyp < kpts.end(); itKeyp++)
		sorted_features.push_back(features.row(keyp_hashes[(*itKeyp).hash()]));

	features = sorted_features.clone();

	// select the first maxFeats features
	if (kpts.size() > maxFeats) {
		vector<KeyPoint> cutKpts(kpts.begin(), kpts.begin() + maxFeats);
		kpts = cutKpts;

		features = features.rowRange(0, maxFeats).clone();
	}

	return 0;

}
Ejemplo n.º 9
0
void mergePoints(const std::vector<cv::Mat> &in_descriptors, const std::vector<cv::Mat> &in_points,
                 cv::Mat &out_descriptors, cv::Mat &out_points) {
  // Figure out the number of points
  size_t n_points = 0, n_images = in_descriptors.size();
  for (size_t image_id = 0; image_id < n_images; ++image_id)
    n_points += in_descriptors[image_id].rows;
  if (n_points == 0)
    return;

  // Fill the descriptors and 3d points
  out_descriptors = cv::Mat(n_points, in_descriptors[0].cols, in_descriptors[0].depth());
  out_points = cv::Mat(1, n_points, CV_32FC3);
  size_t row_index = 0;
  for (size_t image_id = 0; image_id < n_images; ++image_id) {
    // Copy the descriptors
    const cv::Mat & descriptors = in_descriptors[image_id];
    int n_points = descriptors.rows;
    cv::Mat sub_descriptors = out_descriptors.rowRange(row_index, row_index + n_points);
    descriptors.copyTo(sub_descriptors);
    // Copy the 3d points
    const cv::Mat & points = in_points[image_id];
    cv::Mat sub_points = out_points.colRange(row_index, row_index + n_points);
    points.copyTo(sub_points);

    row_index += n_points;
  }
}
Ejemplo n.º 10
0
CV_IMPL int cvFindFundamentalMat( const CvMat* points1, const CvMat* points2,
                                  CvMat* fmatrix, int method,
                                  double param1, double param2, CvMat* _mask )
{
    cv::Mat m1 = cv::cvarrToMat(points1), m2 = cv::cvarrToMat(points2);

    if( m1.channels() == 1 && (m1.rows == 2 || m1.rows == 3) && m1.cols > 3 )
        cv::transpose(m1, m1);
    if( m2.channels() == 1 && (m2.rows == 2 || m2.rows == 3) && m2.cols > 3 )
        cv::transpose(m2, m2);

    const cv::Mat FM = cv::cvarrToMat(fmatrix), mask = cv::cvarrToMat(_mask);
    cv::Mat FM0 = cv::findFundamentalMat(m1, m2, method, param1, param2,
                                         _mask ? cv::_OutputArray(mask) : cv::_OutputArray());

    if( FM0.empty() )
    {
        cv::Mat FM0z = cv::cvarrToMat(fmatrix);
        FM0z.setTo(cv::Scalar::all(0));
        return 0;
    }

    CV_Assert( FM0.cols == 3 && FM0.rows % 3 == 0 && FM.cols == 3 && FM.rows % 3 == 0 && FM.channels() == 1 );
    cv::Mat FM1 = FM.rowRange(0, MIN(FM0.rows, FM.rows));
    FM0.rowRange(0, FM1.rows).convertTo(FM1, FM1.type());
    return FM1.rows / 3;
}
Ejemplo n.º 11
0
cv::Mat Transformations::inv(const cv::Mat &aTb)
{
  // inv(T) = [R^t | -R^t p]
  const cv::Mat R = aTb.rowRange(0,3).colRange(0,3);
  const cv::Mat t = aTb.rowRange(0,3).colRange(3,4);
 
  cv::Mat Rt = R.t();
  cv::Mat t2 = -Rt*t;
  
  cv::Mat ret;
  if(aTb.type() == CV_32F)
  {
    ret = (cv::Mat_<float>(4,4) <<
      Rt.at<float>(0,0), Rt.at<float>(0,1), Rt.at<float>(0,2), t2.at<float>(0,0),
      Rt.at<float>(1,0), Rt.at<float>(1,1), Rt.at<float>(1,2), t2.at<float>(1,0),
      Rt.at<float>(2,0), Rt.at<float>(2,1), Rt.at<float>(2,2), t2.at<float>(2,0),
      0, 0, 0, 1);
  
  }else
  {
    ret = (cv::Mat_<double>(4,4) <<
      Rt.at<double>(0,0), Rt.at<double>(0,1), Rt.at<double>(0,2), t2.at<double>(0,0),
      Rt.at<double>(1,0), Rt.at<double>(1,1), Rt.at<double>(1,2), t2.at<double>(1,0),
      Rt.at<double>(2,0), Rt.at<double>(2,1), Rt.at<double>(2,2), t2.at<double>(2,0),
      0, 0, 0, 1);
  }

  return ret;
}
Ejemplo n.º 12
0
// The image is seperated into two parts, each part's length maximum loc+1;
// x is the seperation line, both two part have x;
float ReidDescriptor::sym_dissimilar_MSKH(int x, cv::Mat img, cv::Mat MSK, int loc, float alpha)
{
	int H = img.rows;
	int W = img.cols;
	int chs = img.channels();

	cv::Mat imgUP = img.rowRange(0, x + 1);//[0,x]
	cv::Mat imgDOWN = img.rowRange(x, img.rows);
	cv::Mat MSK_U = MSK.rowRange(0, x + 1);
	cv::Mat MSK_D = MSK.rowRange(x, MSK.rows);

	int localderU = max(x - loc, 0);
	int localderD = min(loc + 1, MSK_D.rows);

	float ans = -abs(sum(MSK_U.rowRange(localderU, x + 1))[0] - sum(MSK_D.rowRange(0, localderD))[0]);
	return ans;
}
	// out: aligned modelShape
	// in: Rect, ocv with tl x, tl y, w, h (?) and calcs center
	// directly modifies modelShape
	// could move to parent-class
	// assumes mean -0.5, 0.5 and just places inside FB
	cv::Mat alignRigid(cv::Mat modelShape, cv::Rect faceBox) const {
		// we assume we get passed a col-vec. For convenience, we keep it.
		if (modelShape.cols != 1) {
			throw std::runtime_error("The supplied model shape does not have one column (i.e. it doesn't seem to be a column-vector).");
			// We could also check if it's a row-vector and if yes, transpose.
		}
		Mat xCoords = modelShape.rowRange(0, modelShape.rows / 2);
		Mat yCoords = modelShape.rowRange(modelShape.rows / 2, modelShape.rows);		
		// b) Align the model to the current face-box. (rigid, only centering of the mean). x_0
		// Initial estimate x_0: Center the mean face at the [-0.5, 0.5] x [-0.5, 0.5] square (assuming the face-box is that square)
		// More precise: Take the mean as it is (assume it is in a space [-0.5, 0.5] x [-0.5, 0.5]), and just place it in the face-box as
		// if the box is [-0.5, 0.5] x [-0.5, 0.5]. (i.e. the mean coordinates get upscaled)
		xCoords = (xCoords + 0.5f) * faceBox.width + faceBox.x;
		yCoords = (yCoords + 0.5f) * faceBox.height + faceBox.y;

		/*
		// Old algorithm Zhenhua:
		// scale the model:
		double minX, maxX, minY, maxY;
		cv::minMaxLoc(xCoords, &minX, &maxX);
		cv::minMaxLoc(yCoords, &minY, &maxY);
		float faceboxScaleFactor = 1.25f; // 1.25f: value of Zhenhua Matlab FD. Mine: 1.35f
		float modelWidth = maxX - minX;
		float modelHeight = maxY - minY;
		// scale it:
		modelShape = modelShape * (faceBox.width / modelWidth + faceBox.height / modelHeight) / (2.0f * faceboxScaleFactor);
		// translate the model:
		Scalar meanX = cv::mean(xCoords);
		double meanXd = meanX[0];
		Scalar meanY = cv::mean(yCoords);
		double meanYd = meanY[0];
		// move it:
		xCoords += faceBox.x + faceBox.width / 2.0f - meanXd;
		yCoords += faceBox.y + faceBox.height / 1.8f - meanYd; // we use another value for y because we don't want to center the model right in the middle of the face-box
		*/
		return modelShape;
	};
Ejemplo n.º 14
0
void convolution(cv::Mat &inputImg, cv::Mat &outputImg, const cv::Mat &kernel, float scalar) {
    cv::Size imgSize = inputImg.size(); 
    outputImg = cv::Mat(imgSize, CV_8UC1);

    for(int ii = 1; ii < imgSize.width-1; ii++) {
        for(int jj = 1; jj < imgSize.height-1; jj++) {
            auto submat = inputImg.rowRange(ii-1, ii+2).colRange(jj-1, jj+2);
            auto p = single_pixel_convolution(
                submat,
                kernel
            );
            auto it = submat.begin<uchar>();
            auto end = submat.end<uchar>();

            outputImg.at<uchar>(ii,jj) = static_cast<uchar>(p*scalar);
        }
    }
}
Ejemplo n.º 15
0
void drawPose(cv::Mat& img, const cv::Mat& rot, float lineL)
{
	int loc[2] = {70, 70};
	int thickness = 2;
	int lineType  = 8;

	cv::Mat P = (cv::Mat_<float>(3,4) << 
		0, lineL, 0,  0,
		0, 0, -lineL, 0,
		0, 0, 0, -lineL);
	P = rot.rowRange(0,2)*P;
	P.row(0) += loc[0];
	P.row(1) += loc[1];
	cv::Point p0(P.at<float>(0,0),P.at<float>(1,0));

	line(img, p0, cv::Point(P.at<float>(0,1),P.at<float>(1,1)), cv::Scalar( 255, 0, 0 ), thickness, lineType);
	line(img, p0, cv::Point(P.at<float>(0,2),P.at<float>(1,2)), cv::Scalar( 0, 255, 0 ), thickness, lineType);
	line(img, p0, cv::Point(P.at<float>(0,3),P.at<float>(1,3)), cv::Scalar( 0, 0, 255 ), thickness, lineType);
}
Ejemplo n.º 16
0
double DoubleClickFilter::xcorr( const cv::Mat& pattern, const cv::Mat& stream )
{
    // Extract the end-portion of the input stream to match the size of the pattern
    int offset = stream.rows - pattern.rows;
    cv::Mat input( stream.rowRange( cv::Range( offset, stream.rows ) ) );

    // Compute normalizer xcorr between pattern and input
    // The 'pattern' is already centered (0-mean) and has a norm of 1.0
    // which simplify computation.
    //
    // In our case, the normalized xcorr formula is:
    //
    // I = Input, P = pattern
    //
    //      Sum[ (I-mean(I) * P ]
    // r = -----------------------
    //         norm(I-mean(I))

    cv::Mat Im = input - cv::mean( input )[0];

    double r = Im.dot( pattern ) / cv::norm( Im );

    return r;
}
void GeneralTransform::SetTransformationByElements(cv::Mat& transform, const cv::Mat& elements)
{
	int dim_transform = transform.rows;
	cv::Mat identity = cv::Mat::eye(dim_transform, dim_transform, CV_64F);
	transform.rowRange(0, dim_transform - 1) = identity.rowRange(0, dim_transform - 1) + elements.reshape(1, transform_dim_ - 1);
}
void GeneralTransform::SegmentationAndUpdate(std::vector<cv::Mat>& prev_home_cloud, std::vector<cv::Mat>& home_cloud, cv::Mat& query_cloud, cv::Mat& feature, int iteration_count)
{
    // all home cloud suppose to be the whole cloud thus same size...

    /************* nearest neighbor match part *********************/
    
    cv::Mat target_cloud, transformed_cloud;    
    int query_cloud_size = query_cloud.rows;
    int cloud_dim = home_cloud[0].cols;
    std::vector<cv::Mat> indices(num_joints_);
    std::vector<cv::Mat> min_dists(num_joints_);    

    for(int i = 0; i < num_joints_; i++)
    {       
        indices[i] = cv::Mat::zeros(query_cloud_size, 1, CV_32S);
        min_dists[i] = cv::Mat::zeros(query_cloud_size, 1, CV_32F);
    }
    // match different clouds, transformed by different weights...
    // for(int i = 0; i < num_joints_; i++)
    for(int i = 0; i < num_joints_; i++)
    {
        prev_home_cloud[i].convertTo(target_cloud, CV_32F); 
        home_cloud[i].convertTo(transformed_cloud, CV_32F); 
        cv::flann::Index kd_trees(target_cloud, cv::flann::KDTreeIndexParams(4), cvflann::FLANN_DIST_EUCLIDEAN); // build kd tree           
        kd_trees.knnSearch(transformed_cloud, indices[i], min_dists[i], 1, cv::flann::SearchParams(64)); // kd tree search
    }
    // segment the clouds by minimum distance...
    // the two segments are of the same length which is the length of the previous home cloud
    // maybe use vector first and do a whole conversion at the end... that should be good...    
    
    /************* segmentation based on closest neighbor part *********************/
        
    std::vector<int> segmentation_count(num_joints_);
    std::vector<cv::Mat> segmented_target_cloud(num_joints_);
    std::vector<cv::Mat> segmented_transformed_cloud(num_joints_);
    std::vector<cv::Mat> segmented_query_cloud(num_joints_);
    std::vector<cv::Mat> segmented_idx(num_joints_);
    // pre allocate
    for(int i = 0; i < num_joints_; i++)
    {
        segmentation_count[i] = 0; // query_cloud.rows;     
        segmented_idx[i] = cv::Mat::zeros(query_cloud_size, 2, CV_64F); // first column original idx, second column matched idx
    }
    // get the data...
    for(int i = 0; i < query_cloud_size; i++)
    {
        int min_idx = 0;
        double curr_min_dist = min_dists[0].at<float>(i, 0); 
        for(int j = 1; j < num_joints_; j++)
        {
            // find the minimum...
            if(min_dists[j].at<float>(i, 0) < curr_min_dist)
            {
                min_idx = j;
                curr_min_dist = min_dists[j].at<float>(i, 0);
            }
        }       
        int pos = segmentation_count[min_idx];
        segmented_idx[min_idx].at<double>(pos, 0) = i; segmented_idx[min_idx].at<double>(pos, 1) = indices[min_idx].at<int>(i, 0);          
        segmentation_count[min_idx]++;
    }   
    for(int i = 0; i < num_joints_; i++)
    {
        segmented_target_cloud[i] = cv::Mat::zeros(segmentation_count[i], cloud_dim, CV_64F);
        segmented_transformed_cloud[i] = cv::Mat::zeros(segmentation_count[i], cloud_dim, CV_64F);
        segmented_query_cloud[i] = cv::Mat::zeros(segmentation_count[i], cloud_dim, CV_64F);
        for(int j = 0; j < segmentation_count[i]; j++)
        {
            int query_pos = segmented_idx[i].at<double>(j, 0);
            int matched_pos = segmented_idx[i].at<double>(j, 1);
            home_cloud[i].rowRange(query_pos, query_pos + 1).copyTo(segmented_transformed_cloud[i].rowRange(j, j + 1));
            query_cloud.rowRange(query_pos, query_pos + 1).copyTo(segmented_query_cloud[i].rowRange(j, j + 1));
            prev_home_cloud[i].rowRange(matched_pos, matched_pos + 1).copyTo(segmented_target_cloud[i].rowRange(j, j + 1));
        }
    }
    
    /******************* display segmented data... *********************/

    if(iteration_count % 200 == 1)
    {       
        // just display the query cloud...
        std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_segments(num_joints_);   
        for(int i = 0; i < num_joints_; i++)
        {
            if(segmentation_count[i] != 0)
            {
                char cloud_name[10];
                sprintf(cloud_name, "%d", i);
                COLOUR c = GetColour(i * 1.0 / (num_joints_ - 1) * num_joints_, 0, num_joints_);
                cloud_segments[i] = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);                
                Mat2PCD_Trans(segmented_query_cloud[i], cloud_segments[i]);     
                pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color(cloud_segments[i], c.r * 255, c.g * 255, c.b * 255);
                if(iteration_count == 1)
                    viewer_->addPointCloud<pcl::PointXYZ>(cloud_segments[i], cloud_color, cloud_name);
                else
                    viewer_->updatePointCloud<pcl::PointXYZ>(cloud_segments[i], cloud_color, cloud_name);               
            }
        }
        viewer_->spinOnce(1);
    }
    
    /************* weights update part **************/
    // ReOrder_Trans(prev_home_cloud, segmented_target_cloud, indices);
    /*for(int i = 0; i < num_joints_; i++)
        query_cloud.copyTo(segmented_query_cloud[i]);*/
    // CalcGradient(segmented_target_cloud, segmented_transformed_cloud, segmented_query_cloud, feature, segmentation_count);
    // CalcGradient(segmented_target_cloud, segmented_transformed_cloud, segmented_query_cloud, feature, segmentation_count);
    Update(iteration_count);
    
}
Ejemplo n.º 19
0
void FSolver::normalizePoints(const cv::Mat &P, cv::Mat &Q) const
{
  // turn P into homogeneous coords first
  
  if(P.rows == 3) // P is 3xN
  {
    if(P.type() == CV_64F)
      Q = P.clone();
    else
      P.convertTo(Q, CV_64F);
    
    cv::Mat aux = Q.row(0);
    cv::divide(aux, Q.row(2), aux);
    
    aux = Q.row(1);
    cv::divide(aux, Q.row(2), aux);
    
  }
  else if(P.rows == 2) // P is 2xN
  {
    const int N = P.cols;
    
    Q.create(3, N, CV_64F);
    Q.row(2) = 1;
    if(P.type() == CV_64F)
    {
      Q.rowRange(0,2) = P * 1;
    }
    else
    {
      cv::Mat aux = Q.rowRange(0,2);
      P.convertTo(aux, CV_64F);
    }
  }
  else if(P.cols == 3) // P is Nx3
  {
    if(P.type() == CV_64F)
      Q = P.t();
    else
    {
      P.convertTo(Q, CV_64F);
      Q = Q.t();
    } 
    
    cv::Mat aux = Q.row(0);
    cv::divide(aux, Q.row(2), aux);
    
    aux = Q.row(1);
    cv::divide(aux, Q.row(2), aux);
  }
  else if(P.cols == 2) // P is Nx2
  {
    const int N = P.rows;
    
    Q.create(N, 3, CV_64F);
    Q.col(2) = 1;
    cv::Mat aux;
    if(P.type() == CV_64F)
    {
      aux = Q.rowRange(0,2);
      P.rowRange(0,2).copyTo(aux);
    }
    else
    {
      aux = Q.colRange(0,2);
      P.convertTo(aux, CV_64F);
    }
    
    Q = Q.t();
  }  
}
Ejemplo n.º 20
0
void RtToP(const cv::Mat &R, const cv::Mat &t, cv::Mat &P) {
  P = cv::Mat(3, 4, CV_64F);
  R.copyTo(P.rowRange(0, 3).colRange(0, 3));
  t.copyTo(P.rowRange(0, 3).col(3));
}
Ejemplo n.º 21
0
std::vector<Entry> HierClassifier::extractEntries(	cv::Mat imageBGR,
													cv::Mat terrain,
													cv::Mat regionsOnImage,
													cv::Mat maskIgnore,
													int entryWeightThreshold)
{
	using namespace std::chrono;
	high_resolution_clock::time_point start = high_resolution_clock::now();

	if(maskIgnore.empty()){
		maskIgnore = Mat(imageBGR.rows, imageBGR.cols, CV_32SC1, Scalar(0));
	}
	//namedWindow("imageBGR");
	Mat imageHSV;
	cvtColor(imageBGR, imageHSV, CV_BGR2HSV);
	vector<Pixel> pixels;
	//pixels.resize(imageHSV.rows * imageHSV.cols);
	for(int r = 0; r < imageHSV.rows; r++){
		for(int c = 0; c < imageHSV.cols; c++){
			if(maskIgnore.at<int>(r, c) == 0){
				pixels.push_back(Pixel(r, c, regionsOnImage.at<int>(r, c)));
			}
		}
	}
	sort(pixels.begin(), pixels.end());

	vector<pair<int, int> > terrainRegion;
	if(!terrain.empty()){
		Mat terrainPointsImage(terrain.cols, 2, CV_32FC1);
		Mat tmpTerrain = terrain.rowRange(0, 3).t();
		Mat tvec(1, 3, CV_32FC1, Scalar(0));
		Mat rvec(1, 3, CV_32FC1, Scalar(0));
		Mat distCoeffs(1, 5, CV_32FC1, Scalar(0));
		projectPoints(tmpTerrain, tvec, rvec, cameraMatrix, Mat(), terrainPointsImage);
		//terrainPointsImage = terrainPointsImage.t();
		terrainPointsImage = terrainPointsImage.reshape(1).t();
		//cout << tmpTerrain.rowRange(1, 10) << endl;
		//cout << terrainPointsImage.rowRange(1, 10) << endl;
		//cout << cameraMatrix << endl;
		for(int p = 0; p < terrain.cols; p++){
			int imageRow = round(terrainPointsImage.at<float>(1, p));
			int imageCol = round(terrainPointsImage.at<float>(0, p));
			if(imageRow >= 0 && imageRow < imageBGR.rows &&
				imageCol >= 0 && imageCol < imageBGR.cols &&
				maskIgnore.at<int>(imageRow, imageCol) == 0)
			{
				int region = regionsOnImage.at<int>(imageRow, imageCol);
				terrainRegion.push_back(pair<int, int>(region, p));
			}
		}
		sort(terrainRegion.begin(), terrainRegion.end());
	}

	high_resolution_clock::time_point endSorting = high_resolution_clock::now();

	if(debugLevel >= 1){
		cout << "End sorting terrain, terrainRegion.size() = " << terrainRegion.size() << endl;
	}

	vector<Entry> ret;
	int endIm = 0;
	int endTer = 0;
	while(endIm < pixels.size()){
		Mat values, valuesTer, histogramHS, histogramV, statisticsHSV, stisticsLaser;
		int begIm = endIm;
		while(pixels[begIm].imageId == pixels[endIm].imageId){
			endIm++;
			if(endIm == pixels.size()){
				break;
			}
		}
		if(debugLevel >= 1){
			cout << "segment id = " << pixels[begIm].imageId << ", begIm = " << begIm << ", endIm = " << endIm << endl;
		}
		values = Mat(1, endIm - begIm, CV_8UC3);
		for(int p = begIm; p < endIm; p++){
			values.at<Vec3b>(p - begIm) = imageHSV.at<Vec3b>(pixels[p].r, pixels[p].c);
		}

		int begTer = endTer;
		if(begTer < terrainRegion.size()){
			while(terrainRegion[begTer].first != pixels[begIm].imageId){
				begTer++;
				if(begTer >= terrainRegion.size()){
					break;
				}
			}
		}
		endTer = begTer;
		if(endTer < terrainRegion.size()){
			while(terrainRegion[begTer].first == terrainRegion[endTer].first){
				endTer++;
				if(endTer >= terrainRegion.size()){
					break;
				}
			}
		}
		if(endTer - begTer > 0){
			valuesTer = Mat(terrain.rows, endTer - begTer, CV_32FC1);
			Mat tmpImageBGR(imageBGR);
			for(int p = begTer; p < endTer; p++){
				//cout << terrainRegion[p].second << endl;
				terrain.colRange(terrainRegion[p].second, terrainRegion[p].second + 1).copyTo(valuesTer.colRange(p - begTer, p - begTer + 1));
				//cout << "terrainRegion[p].second = " << terrainRegion[p].second << endl;
				//int imageRow = round(terrainPointsImage.at<float>(1 ,terrainRegion[p].second));
				//int imageCol = round(terrainPointsImage.at<float>(0, terrainRegion[p].second));
				//cout << "Point: " << imageRow << ", " << imageCol << endl;
				//tmpImageBGR.at<Vec3b>(imageRow, imageCol) = Vec3b(0x00, 0x00, 0xff);
			}
			//cout << "ImageId = " << pixels[begIm].imageId << endl;
			//imshow("imageBGR", imageBGR);
			//waitKey();
		}
		else{
			//cout << "Warning - no terrain values for imageId " << pixels[begIm].imageId << endl;
			valuesTer = Mat(6, 1, CV_32FC1, Scalar(0));
		}

		if(endIm - begIm > 0){
			int channelsHS[] = {0, 1};
			float rangeH[] = {0, 60};
			float rangeS[] = {0, 256};
			const float* rangesHS[] = {rangeH, rangeS};
			int sizeHS[] = {histHLen, histSLen};
			int channelsV[] = {2};
			float rangeV[] = {0, 256};
			const float* rangesV[] = {rangeV};
			int sizeV[] = {histVLen};
			calcHist(&values, 1, channelsHS, Mat(), histogramHS, 2, sizeHS, rangesHS);
			calcHist(&values, 1, channelsV, Mat(), histogramV, 1, sizeV, rangesV);
			histogramHS = histogramHS.reshape(0, 1);
			histogramV = histogramV.reshape(0, 1);
			normalize(histogramHS, histogramHS);
			normalize(histogramV, histogramV);

			if(debugLevel >= 1){
				cout << "V size = " << histogramV.size() << ", HS size = " << histogramHS.size() << endl;
			}


			values = values.reshape(1, 3);
			//cout << "values size = " << values.size() << endl;
			Mat covarHSV;
			Mat meanHSV;
			calcCovarMatrix(values,
							covarHSV,
							meanHSV,
							CV_COVAR_NORMAL | CV_COVAR_SCALE | CV_COVAR_COLS,
							CV_32F);

			if(debugLevel >= 1){
				cout << "Calculated covar matrix" << endl;
			}
			covarHSV = covarHSV.reshape(0, 1);
			meanHSV = meanHSV.reshape(0, 1);
			//normalize(covarHSV, covarHSV);
			//normalize(meanHSV, meanHSV);

			Mat covarLaser, meanLaser;
			calcCovarMatrix(valuesTer.rowRange(4, 6),
							covarLaser,
							meanLaser,
							CV_COVAR_NORMAL | CV_COVAR_SCALE | CV_COVAR_COLS,
							CV_32F);
			covarLaser = covarLaser.reshape(0, 1);
			meanLaser = meanLaser.reshape(0, 1);
			//normalize(covarLaser, covarLaser);
			//normalize(meanLaser, meanLaser);
			//cout << "covarLaser = " << covarLaser << endl;
			//cout << "meanLaser = " << meanLaser << endl;

			//cout << "Entry " << ret.size() << endl;
			//cout << "histHS = " << histogramHS << endl;
			//cout << "histV = " << histogramV << endl;
			//cout << "covarHSV = " << covarHSV << endl;
			//cout << "meanHSV = " << meanHSV << endl;

			Mat kurtLaser(1, 2, CV_32FC1);
			Mat tmpVal;
			valuesTer.rowRange(4, 6).copyTo(tmpVal);
			//cout << "tmpVal = " << tmpVal << endl;
			//cout << "mean(0) = " << meanLaser.at<float>(0) << ", mean(1) = " << meanLaser.at<float>(1) << endl;
			//cout << "stdDev^4(0) = " << pow(covarLaser.at<float>(0), 2) << ", stdDev^4(3) = " << pow(covarLaser.at<float>(3), 2) << endl;
			tmpVal.rowRange(0, 1) -= meanLaser.at<float>(0);
			tmpVal.rowRange(1, 2) -= meanLaser.at<float>(1);

			pow(tmpVal, 4, tmpVal);
			kurtLaser.at<float>(0) = sum(tmpVal.rowRange(0, 1))(0);
			if(tmpVal.cols * pow(covarLaser.at<float>(0), 2) != 0){
				kurtLaser.at<float>(0) = kurtLaser.at<float>(0) / (tmpVal.cols * pow(covarLaser.at<float>(0), 2)) - 3;
			}
			kurtLaser.at<float>(1) = sum(tmpVal.rowRange(1, 2))(0);
			if(tmpVal.cols * pow(covarLaser.at<float>(3), 2) != 0){
				kurtLaser.at<float>(1) = kurtLaser.at<float>(1) / (tmpVal.cols * pow(covarLaser.at<float>(3), 2)) - 3;
			}

			Mat histogramDI;
			int channelsDI[] = {0, 1};
			float rangeD[] = {1500, 2500};
			float rangeI[] = {1800, 4000};
			const float* rangesDI[] = {rangeD, rangeI};
			int sizeDI[] = {histDLen, histILen};
			Mat valHistD = valuesTer.rowRange(4, 5);
			Mat valHistI = valuesTer.rowRange(5, 6);
			Mat valuesHistDI[] = {valHistD, valHistI};
			calcHist(valuesHistDI, 2, channelsDI, Mat(), histogramDI, 2, sizeDI, rangesDI);
			histogramDI = histogramDI.reshape(0, 1);
			normalize(histogramDI, histogramDI);

			//cout << "histogramDI = " << histogramDI << endl;

			Entry tmp;
			tmp.imageId = pixels[begIm].imageId;
			tmp.weight = (endIm - begIm)/* + (endTer - begTer)*/;
			tmp.descriptor = Mat(1, histHLen*histSLen +
								histVLen +
								meanHSVLen +
								covarHSVLen +
								histDLen*histILen +
								meanLaserLen +
								covarLaserLen,
								CV_32FC1);

			int begCol = 0;
			histogramHS.copyTo(tmp.descriptor.colRange(begCol, begCol + histHLen*histSLen));
			begCol += histHLen*histSLen;
			histogramV.copyTo(tmp.descriptor.colRange(begCol, begCol + histVLen));
			begCol += histVLen;
			meanHSV.copyTo(tmp.descriptor.colRange(begCol, begCol + meanHSVLen));
			begCol += meanHSVLen;
			covarHSV.copyTo(tmp.descriptor.colRange(begCol, begCol + covarHSVLen));
			begCol += covarHSVLen;
			histogramDI.copyTo(tmp.descriptor.colRange(begCol, begCol + histDLen*histILen));
			begCol += histDLen*histILen;
			meanLaser.copyTo(tmp.descriptor.colRange(begCol, begCol + meanLaserLen));
			begCol += meanLaserLen;
			covarLaser.copyTo(tmp.descriptor.colRange(begCol, begCol + covarLaserLen));
			begCol += covarLaserLen;
			//kurtLaser.copyTo(tmp.descriptor.colRange(begCol, begCol + kurtLaserLen));
			//begCol += kurtLaserLen;
			//cout << "descriptor = " << tmp.descriptor << endl;

			if(endIm - begIm > entryWeightThreshold){
				ret.push_back(tmp);
			}
		}
	}

	static duration<double> sortingTime = duration<double>::zero();
	static duration<double> compTime = duration<double>::zero();
	static duration<double> wholeTime = duration<double>::zero();
	static int times = 0;

	high_resolution_clock::time_point endComp = high_resolution_clock::now();
	sortingTime += duration_cast<duration<double> >(endSorting - start);
	compTime += duration_cast<duration<double> >(endComp - endSorting);
	wholeTime += duration_cast<duration<double> >(endComp - start);
	times++;
	if(debugLevel >= 1){
		cout << "Times: " << times << endl;
		cout << "Extract Average sorting time: " << sortingTime.count()/times << endl;
		cout << "Extract Average computing time: " << compTime.count()/times << endl;
		cout << "Extract Average whole time: " << wholeTime.count()/times << endl;
	}

	return ret;
}
cv::Mat homography2affine(const cv::Mat &homography)
{
    return homography.rowRange(0, 2).clone();
}
void GeneralTransform::SegmentationAndUpdateFixedHomePos(std::vector<cv::Mat>& home_cloud, cv::Mat& query_cloud, cv::Mat& feature, int iteration_count)
{
    cv::Mat target_cloud, transformed_cloud;    
    int query_cloud_size = query_cloud.rows;
    int cloud_dim = home_cloud[0].cols;
    std::vector<cv::Mat> indices(num_joints_);
    std::vector<cv::Mat> min_dists(num_joints_);    
    int p_rates = 1e-1;

    // match different clouds, transformed by different weights, with the home cloud template...
	cv::flann::Index kd_trees(home_cloud_template_float_, cv::flann::KDTreeIndexParams(4), cvflann::FLANN_DIST_EUCLIDEAN); // build kd tree 
    for(int i = 0; i < num_joints_; i++)
    {       
        indices[i] = cv::Mat::zeros(query_cloud_size, 1, CV_32S);
        min_dists[i] = cv::Mat::zeros(query_cloud_size, 1, CV_32F);
        home_cloud[i].convertTo(transformed_cloud, CV_32F); 		        
        kd_trees.knnSearch(transformed_cloud, indices[i], min_dists[i], 1, cv::flann::SearchParams(64)); // kd tree search
    }

    /************* segmentation based on closest neighbor and update the probability according to distance *********************/

    // first accumulate the data...    
    for(int i = 0; i < query_cloud_size; i++)
    {
		int curr_idx_0 = indices[0].at<int>(i, 0);
		int curr_idx_1 = indices[1].at<int>(i, 0);
		// two joints here
		if(min_dists[0].at<float>(i, 0) < min_dists[1].at<float>(i, 0))
			vote_accumulation_.at<double>(curr_idx_0, 0) = vote_accumulation_.at<double>(curr_idx_0, 0) + 1;
		else
			vote_accumulation_.at<double>(curr_idx_0, 1) = vote_accumulation_.at<double>(curr_idx_0, 1) + 1;        
    }

    for(int i = 0; i < home_cloud_template_.rows; i++)
    {
		if(vote_accumulation_.at<double>(i, 0) == 0 && vote_accumulation_.at<double>(i, 1) == 0)
		{
			home_cloud_label_.at<double>(i, 0) = 0.5; home_cloud_label_.at<double>(i, 1) = 0.5;
		}
		else if(vote_accumulation_.at<double>(i, 0) == 0)
		{
			home_cloud_label_.at<double>(i, 0) = 0; home_cloud_label_.at<double>(i, 1) = 1;
		}
		else if(vote_accumulation_.at<double>(i, 1) == 0)
		{
			home_cloud_label_.at<double>(i, 0) = 1; home_cloud_label_.at<double>(i, 1) = 0;
		}
		else
		{
			double sum = vote_accumulation_.at<double>(i, 0) + vote_accumulation_.at<double>(i, 1);
			home_cloud_label_.at<double>(i, 0) = vote_accumulation_.at<double>(i, 0) / sum;
			home_cloud_label_.at<double>(i, 1) = vote_accumulation_.at<double>(i, 1) / sum;
		}       
    }

    // home_cloud_label_ = home_cloud_label_ + p_rates * (curr_probability - home_cloud_label_);

	if(iteration_count % 500 == 1)
    {       
        // just display the query cloud...		
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_template;        
		colored_template.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
		// colored_template->resize(home_cloud_template_.rows);
		for(int i = 0; i < home_cloud_template_.rows; i++)
		{
			pcl::PointXYZRGB point;
			point.x = home_cloud_template_.at<double>(i, 0);
			point.y = home_cloud_template_.at<double>(i, 1);
			point.z = home_cloud_template_.at<double>(i, 2);
			COLOUR c = GetColour(home_cloud_label_.at<double>(i, 0), 0.0, 1.0);
			point.r = c.r * 255.0;
			point.g = c.g * 255.0;
			point.b = c.b * 255.0;

			colored_template->push_back(point);						
		}                
		pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(colored_template);
        if(iteration_count == 1)
			viewer_->addPointCloud<pcl::PointXYZRGB>(colored_template, rgb, "template");        
        else
            viewer_->updatePointCloud<pcl::PointXYZRGB>(colored_template, rgb, "template");
        
        viewer_->spinOnce(10);
    }

	std::vector<int> segmentation_count(num_joints_);
    std::vector<cv::Mat> segmented_target_cloud(num_joints_);
    std::vector<cv::Mat> segmented_transformed_cloud(num_joints_);
    std::vector<cv::Mat> segmented_query_cloud(num_joints_);
    std::vector<cv::Mat> segmented_idx(num_joints_);
    // pre allocate
	std::cout << "pre-allocate..." << std::endl;
    for(int i = 0; i < num_joints_; i++)
    {
        segmentation_count[i] = 0; // query_cloud.rows;     
        segmented_idx[i] = cv::Mat::zeros(query_cloud_size, 2, CV_64F); // first column original idx, second column matched idx
    }
    // get the data... only work for two joints here...
	std::cout << "segment..." << std::endl;
    for(int i = 0; i < query_cloud_size; i++)
    {
        int min_idx = 0;
		// this line has bug...
		if(home_cloud_label_.at<double>(i, 0) > home_cloud_label_.at<double>(i, 1))
			min_idx = 0;
		else
			min_idx = 1;
        int pos = segmentation_count[min_idx];
        segmented_idx[min_idx].at<double>(pos, 0) = i; 
		segmented_idx[min_idx].at<double>(pos, 1) = indices[min_idx].at<int>(i, 0);          
        segmentation_count[min_idx]++;
    }   
	// update...
	std::cout << "separate data..." << std::endl;
    for(int i = 0; i < num_joints_; i++)
    {
        segmented_target_cloud[i] = cv::Mat::zeros(segmentation_count[i], cloud_dim, CV_64F);
        segmented_transformed_cloud[i] = cv::Mat::zeros(segmentation_count[i], cloud_dim, CV_64F);
        segmented_query_cloud[i] = cv::Mat::zeros(segmentation_count[i], cloud_dim, CV_64F);
        for(int j = 0; j < segmentation_count[i]; j++)
        {
            int query_pos = segmented_idx[i].at<double>(j, 0);
            int matched_pos = segmented_idx[i].at<double>(j, 1);
			if(matched_pos >= home_cloud_template_.rows || j >= segmented_transformed_cloud[i].rows || query_pos >= home_cloud[i].rows)
				std::cout << "matched pos not correct..." << std::endl;
            home_cloud[i].rowRange(query_pos, query_pos + 1).copyTo(segmented_transformed_cloud[i].rowRange(j, j + 1));
            query_cloud.rowRange(query_pos, query_pos + 1).copyTo(segmented_query_cloud[i].rowRange(j, j + 1));
			home_cloud_template_.rowRange(matched_pos, matched_pos + 1).copyTo(segmented_target_cloud[i].rowRange(j, j + 1));
        }
    }


    // CalcGradient(matched_template, home_cloud, query_cloud_list, feature, matched_probabilities);
	std::cout << "calculating gradient..." << std::endl;
	// CalcGradient(segmented_target_cloud, segmented_transformed_cloud, segmented_query_cloud, feature, segmentation_count);
	std::cout << "updating..." << std::endl;
    Update(iteration_count);
}
	// out: aligned modelShape
	// in: Rect, ocv with tl x, tl y, w, h (?) and calcs center
	// directly modifies modelShape
	// could move to parent-class
	// assumes mean -0.5, 0.5 and just places inside FB
	// TODO: Actually this function uses model.mean as well as a modelShape input, this is
	// a big ambiguous. Move this function out of this class? But we need access to getLandmarkAsPoint?
	// Also think about the alignRigid function above.
	// @throws ...exception When we can't align (e.g. the given landmarks are 2 eyes that are on top of each other, so sx and sy are not calculable).
	cv::Mat alignRigid(cv::Mat modelShape, imageio::LandmarkCollection landmarks) const {
		// we assume we get passed a col-vec. For convenience, we keep it.
		if (modelShape.cols != 1) {
			throw std::runtime_error("The supplied model shape does not have one column (i.e. it doesn't seem to be a column-vector).");
			// We could also check if it's a row-vector and if yes, transpose.
		}
		Mat xCoords = modelShape.rowRange(0, modelShape.rows / 2);
		Mat yCoords = modelShape.rowRange(modelShape.rows / 2, modelShape.rows);
		
		Mat modelLandmarksX, modelLandmarksY, alignmentLandmarksX, alignmentLandmarksY;
		for (auto&& lm : landmarks.getLandmarks()) {
			cv::Point2f p;
			// What follows is some ugly hackery because the eye-center points from PaSC are not defined in the
			// model. Find a more generic solution for this! As it is really a special case, maybe add a flag parameter to this function?
			// We create a landmark-point in the model at the eye centers that doesn't exist, and use this one to align the model
			if (lm->getName() == "le") {
				cv::Point2f reye_oc = model.getLandmarkAsPoint("37"); // Note: The points might not exist in the model
				cv::Point2f reye_ic = model.getLandmarkAsPoint("40");
				cv::Point2f reye_center = cv::Vec2f(reye_oc + reye_ic) / 2.0f;
				p = reye_center;
			}
			else if (lm->getName() == "re") {
				cv::Point2f leye_oc = model.getLandmarkAsPoint("46");
				cv::Point2f leye_ic = model.getLandmarkAsPoint("43");
				cv::Point2f leye_center = cv::Vec2f(leye_oc + leye_ic) / 2.0f;
				p = leye_center;
			}
			else {
				p = model.getLandmarkAsPoint(lm->getName());
			}
			modelLandmarksX.push_back(p.x);
			modelLandmarksY.push_back(p.y);
			alignmentLandmarksX.push_back(lm->getX());
			alignmentLandmarksY.push_back(lm->getY());
		}

		// Note: Calculate the scaling first, then scale, then calculate the translation.
		// Because the translation will change once we scale the model (the centroid of our
		// two points is not at the centroid of the whole model (which is the point from where we scale). (ZF)
		float sx = calculateScaleRatio(modelLandmarksX, alignmentLandmarksX);
		float sy = calculateScaleRatio(modelLandmarksY, alignmentLandmarksY);
		float s;
		// Note: If the y-difference is very small (instead of zero), the sx or sy number could be
		// very large. This could cause side-effects?
		// 'isnormal': Determines if the given floating point number arg is normal, i.e. is neither zero, subnormal, infinite, nor NaN.
		if (!std::isnormal(sx) && !std::isnormal(sy)) {
			// sx and sy are both not calculable, i.e. we can't align (e.g. the given landmarks are 2 eyes that are on top of each other).
			// This happens in very rare cases (1 image so far on PaSC)
			throw std::runtime_error("x- and y-scale both not calculable, cannot align the model."); // we should use a derived exception here
		}
		// Now at least one of sx and sy is normal:
		if (!std::isnormal(sx)) { 
			s = sy;
		}
		else if (!std::isnormal(sy)) {
			s = sx;
		}
		else {
			s = (sx + sy) / 2.0f;
		}

		modelLandmarksX *= s;
		modelLandmarksY *= s;
		float tx = calculateMeanTranslation(modelLandmarksX, alignmentLandmarksX);
		float ty = calculateMeanTranslation(modelLandmarksY, alignmentLandmarksY);

		xCoords = (xCoords * s + tx);
		yCoords = (yCoords * s + ty);

		return modelShape;
	};
Ejemplo n.º 25
0
// Given the image of a person, get the axes, and return the map of kernel;
cv::Mat ReidDescriptor::map_kernel(cv::Mat img, int delta1, int delta2, float alpha, float varW, int H, int W, int &TLanti, int &HDanti,cv::Mat MSK)
{
	// UPLOADING AND FEATURE EXTRACTION
	if(MSK.empty()) {
		MSK = cv::Mat::ones(cv::Size(W, H), CV_8U);
	}

	// (me) Wouldn't it be quicker to just create a image of the same size rather than clone it ?
	cv::Mat img_hsv = img.clone();
	// Matlab:0:1,0:1,0:1 ; Opencv:0:255,0:255,0:180
	cv::Mat img_cielab = img.clone();
	// Matlab:0:255,0:255,0:255 ; Opencv:0:255,0:255,0:255
	cvtColor(img, img_hsv, CV_BGR2HSV);
	cvtColor(img, img_cielab, CV_BGR2Lab);

	// (me) Normalisation...?
	vector<cv::Mat> temp;
	split(img_hsv, temp);
	temp[0].convertTo(temp[0], CV_32FC1, 1 / 180.f);
	temp[1].convertTo(temp[1], CV_32FC1, 1 / 255.f);
	temp[2].convertTo(temp[2], CV_32FC1, 1 / 255.f);
	merge(temp, img_hsv);

	//double m, M;
	//cv::minMaxLoc(img_hsv,&m,&M);
	//imshow("imghsv",temp[0]);
	//cv::normalize(img_hsv,img_hsv,CV_MINMAX);

	TLanti = fminbnd(dissym_div, img_hsv, MSK, delta1, alpha, delta1, H - delta1);
	//cout << dissym_div(33, img_hsv, MSK, delta1, alpha)<<endl;
	//cout << dissym_div(34, img_hsv, MSK, delta1, alpha) << endl;
	int BUsim = fminbnd(sym_div, img_hsv.rowRange(0, img_hsv.rows), MSK.rowRange(0, img_hsv.rows), delta2, alpha, delta2, W - delta2);
	//cout << sym_div(18, img_hsv.rowRange(0, TLanti + 1), MSK.rowRange(0, TLanti + 1), delta2, alpha) << endl;
	//cout << sym_div(35, img_hsv.rowRange(0, TLanti + 1), MSK.rowRange(0, TLanti + 1), delta2, alpha) << endl;
	//int LEGsim = fminbnd(sym_div, img_hsv.rowRange(TLanti, img_hsv.rows), MSK.rowRange(TLanti, MSK.rows), delta2, alpha, delta2, W - delta2);
	HDanti = fminbnd(sym_dissimilar_MSKH, img_hsv, MSK, delta1, 0, 5, TLanti);

	//cv::Mat img_show = img.clone();
	//line(img_show, cv::Point(0, TLanti), cv::Point(W, TLanti), cv::Scalar(255, 0, 0));
	//line(img_show, cv::Point(0, HDanti), cv::Point(W, HDanti), cv::Scalar(255, 0, 0));
	//line(img_show, cv::Point(BUsim, HDanti), cv::Point(BUsim, TLanti), cv::Scalar(255, 0, 0));
	//line(img_show, cv::Point(LEGsim, TLanti), cv::Point(LEGsim, H), cv::Scalar(255, 0, 0));
	//imshow("test", img_show);
	//cv::waitKey(0);

	// Kernel - map computation
	vector<cv::Mat> img_split;
	split(img_hsv, img_split);
	img_split[2].convertTo(img_split[2], CV_8UC1, 255);
	equalizeHist(img_split[2], img_split[2]);
	img_split[2].convertTo(img_split[2], CV_32FC1, 1.0 / 255);
	merge(img_split, img_hsv);

	//cv::Mat HEADW = cv::Mat::zeros(cv::Size(W, HDanti + 1), CV_32FC1);

	//cv::Mat UP = img_hsv.rowRange(HDanti + 1, TLanti + 1);
	cv::Mat UP = img_hsv.rowRange(0, img_hsv.rows);
	cv::Mat UPW = gau_kernel(BUsim, varW, UP.rows, W);

	//cv::Mat DOWN = img_hsv.rowRange(TLanti + 1, img_hsv.rows);
	//cv::Mat DOWNW = gau_kernel(LEGsim, varW, DOWN.rows, W);

	cv::Mat MAP_KRNL = UPW / max_value(UPW);
	//cv::Mat MAP_KRNL = HEADW.clone() / max_value(HEADW);
	//MAP_KRNL.push_back(cv::Mat(UPW / max_value(UPW)));
	//MAP_KRNL.push_back(cv::Mat(DOWNW / max_value(DOWNW)));

	if (H - MAP_KRNL.rows > 0) {
		MAP_KRNL = padarray(MAP_KRNL, H - MAP_KRNL.rows);
	} else {
		MAP_KRNL = MAP_KRNL.rowRange(0, H);
	}

	//cv::imshow("map_kernel", MAP_KRNL);
	//cv::waitKey(0);
	return MAP_KRNL;
}
Ejemplo n.º 26
0
static void initMorePoints(const cv::Mat &img_l, const cv::Mat &img_r, std::vector<int> &updateVect, std::vector<cv::Point2f> &z_all_l,
        std::vector<cv::Point2f> &z_all_r, int stereo) {
    if (!img_l.data)
        throw "Left image is invalid";
    if (stereo && !img_r.data)
        throw "Right image is invalid";

    unsigned int targetNumPoints = 0;
    // count the features that need to be initialized
    for (int i = 0; i < updateVect.size(); i++) {
        if (updateVect[i] == 2)  // 2 means new feature requested
            targetNumPoints++;
    }

    if (!targetNumPoints)
        return;

    std::vector<cv::KeyPoint> keypointsL, keypointsR, goodKeypointsL, unusedKeypoints;
    cv::Mat descriptorsL, descriptorsR;

    int numBinsX = 4;
    int numBinsY = 4;
    int binWidth = img_l.cols / numBinsX;
    int binHeight = img_l.rows / numBinsY;
    int targetFeaturesPerBin = (updateVect.size() - 1) / (numBinsX * numBinsY) + 1;  // total number of features that should be in each bin

    std::vector<std::vector<int> > featuresPerBin(numBinsX, std::vector<int>(numBinsY, 0));

    // count the number of active features in each bin
    for (int i = 0; i < prev_corners.size(); i++) {
        if (updateVect[i] == 1) {
            int binX = prev_corners[i].x / binWidth;
            int binY = prev_corners[i].y / binHeight;

            if (binX >= numBinsX) {
                printf("Warning: writing to binX out of bounds: %d >= %d\n", binX, numBinsX);
                continue;
            }
            if (binY >= numBinsY) {
                printf("Warning: writing to binY out of bounds: %d >= %d\n", binY, numBinsY);
                continue;
            }

            featuresPerBin[binX][binY]++;
        }
    }

    unsigned int dist = binWidth / targetFeaturesPerBin;
    // go through each cell and detect features
    for (int x = 0; x < numBinsX; x++) {
        for (int y = 0; y < numBinsY; y++) {
            int neededFeatures = std::max(0, targetFeaturesPerBin - featuresPerBin[x][y]);

            if (neededFeatures) {
                int col_from = x * binWidth;
                int col_to = std::min((x + 1) * binWidth, img_l.cols);
                int row_from = y * binHeight;
                int row_to = std::min((y + 1) * binHeight, img_l.rows);

                std::vector<cv::KeyPoint> keypoints, goodKeypointsBin;
                FAST(img_l.rowRange(row_from, row_to).colRange(col_from, col_to), keypoints, 10);

                sort(keypoints.begin(), keypoints.end(), compareKeypoints);

                // add bin offsets to the points
                for (int i = 0; i < keypoints.size(); i++) {
                    keypoints[i].pt.x += col_from;
                    keypoints[i].pt.y += row_from;
                }

                // check if the new features are far enough from existing points
                int newPtIdx = 0;
                for (; newPtIdx < keypoints.size(); newPtIdx++) {
                    int new_pt_x = keypoints[newPtIdx].pt.x;
                    int new_pt_y = keypoints[newPtIdx].pt.y;

                    bool far_enough = true;
                    for (int j = 0; j < prev_corners.size(); j++) {
                        if (prev_status[j] == 0)
                            continue;
                        int existing_pt_x = prev_corners[j].x;
                        int existing_pt_y = prev_corners[j].y;
                        if (abs(existing_pt_x - new_pt_x) < dist && abs(existing_pt_y - new_pt_y) < dist) {
                            far_enough = false;
                            unusedKeypoints.push_back(keypoints[newPtIdx]);
                            break;
                        }
                    }
                    if (far_enough) {
                        // check if the new feature is too close to a new one
                        for (int j = 0; j < goodKeypointsBin.size(); j++) {
                            int existing_pt_x = goodKeypointsBin[j].pt.x;
                            int existing_pt_y = goodKeypointsBin[j].pt.y;
                            if (abs(existing_pt_x - new_pt_x) < dist && abs(existing_pt_y - new_pt_y) < dist) {
                                far_enough = false;
                                unusedKeypoints.push_back(keypoints[newPtIdx]);
                                break;
                            }
                        }
                        if (far_enough) {
                            goodKeypointsBin.push_back(keypoints[newPtIdx]);
                            if (goodKeypointsBin.size() == neededFeatures)
                                break;
                        }
                    }
                }
                // insert the good points into the vector containing the new points of the whole image
                goodKeypointsL.insert(goodKeypointsL.end(), goodKeypointsBin.begin(), goodKeypointsBin.end());
                // save the unused keypoints for later
                if (newPtIdx < keypoints.size() - 1) {
                    unusedKeypoints.insert(unusedKeypoints.end(), keypoints.begin() + newPtIdx, keypoints.end());
                }
            }
        }
    }

    // if not many features were requested, we may have found too many features. delete from all bins for equal distancing
    if (goodKeypointsL.size() > targetNumPoints) {
        int numFeaturesToRemove = goodKeypointsL.size() - targetNumPoints;
        int stepSize = targetNumPoints / numFeaturesToRemove + 2;  // make sure the step size is big enough so we dont remove too many features

        std::vector<cv::KeyPoint> goodKeypointsL_shortened;
        for (int i = 0; i < goodKeypointsL.size(); i++) {
            if (i % stepSize) {
                goodKeypointsL_shortened.push_back(goodKeypointsL[i]);
            }
        }
        goodKeypointsL = goodKeypointsL_shortened;
    }

    if (goodKeypointsL.size() < targetNumPoints) {
        // try to insert new points that were not used in the bins
        sort(unusedKeypoints.begin(), unusedKeypoints.end(), compareKeypoints);

        dist /= 2;  // reduce the distance criterion

        for (int newPtIdx = 0; newPtIdx < unusedKeypoints.size(); newPtIdx++) {
            int new_pt_x = unusedKeypoints[newPtIdx].pt.x;
            int new_pt_y = unusedKeypoints[newPtIdx].pt.y;

            bool far_enough = true;
            for (int j = 0; j < prev_corners.size(); j++) {
                if (prev_status[j] == 0)
                    continue;
                int existing_pt_x = prev_corners[j].x;
                int existing_pt_y = prev_corners[j].y;
                if (abs(existing_pt_x - new_pt_x) < dist && abs(existing_pt_y - new_pt_y) < dist) {
                    far_enough = false;
                    break;
                }
            }
            if (far_enough) {
                // check if the new feature is too close to a new one
                for (int j = 0; j < goodKeypointsL.size(); j++) {
                    int existing_pt_x = goodKeypointsL[j].pt.x;
                    int existing_pt_y = goodKeypointsL[j].pt.y;
                    if (abs(existing_pt_x - new_pt_x) < dist && abs(existing_pt_y - new_pt_y) < dist) {
                        far_enough = false;
                        break;
                    }
                }
                if (far_enough) {
                    goodKeypointsL.push_back(unusedKeypoints[newPtIdx]);
                    if (goodKeypointsL.size() == targetNumPoints)
                        break;
                }
            }
        }
    }

    if (goodKeypointsL.empty()) {
        for (int i = 0; i < updateVect.size(); i++) {
            if (updateVect[i] == 2)
                updateVect[i] = 0;
        }
        return;
    }

    std::vector<cv::Point2f> leftPoints, rightPoints;

    if (stereo) {
        if (!stereoMatchOpticalFlow(img_l, img_r, goodKeypointsL, leftPoints, rightPoints)) {
            for (int i = 0; i < updateVect.size(); i++) {
                if (updateVect[i] == 2)
                    updateVect[i] = 0;
            }
            return;
        }
        if (leftPoints.size() != rightPoints.size())  // debug
            debug_msg_count ++;
            if (debug_msg_count % 50 == 0) {
                printf("Left and right points have different sizes: left %d, right %d\n", (int) leftPoints.size(), (int) rightPoints.size());
            }
    } else {
        leftPoints.resize(goodKeypointsL.size());
        for (int i = 0; i < goodKeypointsL.size(); i++)
        {
            leftPoints[i] = goodKeypointsL[i].pt;
        }
    }
    if (leftPoints.size() < targetNumPoints)
        debug_msg_count ++;
        if (debug_msg_count % 50 == 0) {
            printf("Number of good matches: %d, desired: %d\n", (int) leftPoints.size(), targetNumPoints);
        }

    if (prev_corners.size() < updateVect.size())
        prev_corners.resize(updateVect.size());
    int matches_idx = 0;
    for (int i = 0; i < updateVect.size(); i++) {
        if (updateVect[i] == 2) {
            if (matches_idx < leftPoints.size()) {
                prev_corners[i] = leftPoints[matches_idx];
                prev_status[i] = 1;

                z_all_l[i] = leftPoints[matches_idx];

                if (stereo) {
                    z_all_r[i] = rightPoints[matches_idx];
                }

                matches_idx++;
            } else {
                updateVect[i] = 0;
            }
        }
    }
}