// 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)); } }
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); }
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); }
// 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; }
// 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); }
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; }
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; } }
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; }
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; }
// 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; };
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); } } }
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); }
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); }
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(); } }
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)); }
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; };
// 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; }
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; } } } }