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