void sharpen(const cv::Mat& image, cv::Mat& result){
// allocate if necessary
result.create(global::image.size(), global::image.type());
std::cout << "Size: " << global::image.size() << std::endl;
std::cout << "Cols: " << global::image.cols << "\n" << "Rows: " << global::image.rows << std::endl;
for (int j=1; j<global::image.rows-1; j++){ // for all rows except first and last
const uchar* previous = global::image.ptr<const uchar>(j-1); // previous row
const uchar* current = global::image.ptr<const uchar>(j); // current row
const uchar* next = global::image.ptr<const uchar>(j+1); // next row
uchar* output = result.ptr<uchar>(j); // output row
for (int i=1; i<global::image.cols-1; i++){
*output = cv::saturate_cast<uchar>(5*current[i]-current[i-1]-current[i+1]-previous[i]-next[i]);
output++;
}
}
// set unprocessed pixels to 0
result.row(0).setTo(cv::Scalar(0));
result.row(result.rows-1).setTo(cv::Scalar(0));
result.col(0).setTo(cv::Scalar(0));
result.col(result.cols-1).setTo(cv::Scalar(0));
}
/* This function will rearrange dataset for training in a random order. This step is
* necessary to make training more accurate.
*/
void LetterClassifier::ShuffleDataset(cv::Mat &training_data, cv::Mat &label_mat, int numIter)
{
/* initialize random seed: */
srand(time(NULL));
int x = 0, y = 0;
assert(training_data.cols == label_mat.rows);
int numData = training_data.cols;
if (numIter <= 0)
numIter = numData;
if (training_data.type() != CV_32FC1)
training_data.convertTo(training_data, CV_32FC1);
cv::Mat temp_data_mat(training_data.rows, 1, CV_32FC1);
cv::Mat temp_label_mat(1, 1, CV_32FC1);
// Interate 'numIter' to rearrange dataset
for (int n = 0; n < numIter; n++)
{
x = (rand() % numData);
y = (rand() % numData);
// swap data
training_data.col(x).copyTo(temp_data_mat.col(0));
training_data.col(y).copyTo(training_data.col(x));
temp_data_mat.col(0).copyTo(training_data.col(y));
// swap label
label_mat.row(x).copyTo(temp_label_mat.row(0));
label_mat.row(y).copyTo(label_mat.row(x));
temp_label_mat.row(0).copyTo(label_mat.row(y));
}
}
// Evaluates a Gaussian kernel with bandwidth SIGMA for all relative shifts between input images X and Y, which must both be MxN. They must also be periodic (ie., pre-processed with a cosine window).
cv::Mat KCFTracker::gaussianCorrelation(cv::Mat x1, cv::Mat x2)
{
using namespace FFTTools;
cv::Mat c = cv::Mat( cv::Size(size_patch[1], size_patch[0]), CV_32F, cv::Scalar(0) );
// HOG features
if (_hogfeatures) {
cv::Mat caux;
cv::Mat x1aux;
cv::Mat x2aux;
for (int i = 0; i < size_patch[2]; i++) {
x1aux = x1.row(i); // Procedure do deal with cv::Mat multichannel bug
x1aux = x1aux.reshape(1, size_patch[0]);
x2aux = x2.row(i).reshape(1, size_patch[0]);
cv::mulSpectrums(fftd(x1aux), fftd(x2aux), caux, 0, true);
caux = fftd(caux, true);
rearrange(caux);
caux.convertTo(caux,CV_32F);
c = c + real(caux);
}
}
// Gray features
else {
cv::mulSpectrums(fftd(x1), fftd(x2), c, 0, true);
c = fftd(c, true);
rearrange(c);
c = real(c);
}
cv::Mat d;
cv::max(( (cv::sum(x1.mul(x1))[0] + cv::sum(x2.mul(x2))[0])- 2. * c) / (size_patch[0]*size_patch[1]*size_patch[2]) , 0, d);
cv::Mat k;
cv::exp((-d / (sigma * sigma)), k);
return k;
}
// Eigenvectors as rows
void projectToEigenvectors( const RLearning::PCA &pcaPos, const cv::Mat &evecsRowsPos,
const RLearning::PCA &pcaNeg, const cv::Mat &evecsRowsNeg)
{
cv::Mat selEvecsRowsPos( 2, evecsRowsPos.cols, evecsRowsPos.type());
cv::Mat selEvecsRowsNeg( 2, evecsRowsNeg.cols, evecsRowsNeg.type());
evecsRowsPos.row(0).copyTo( selEvecsRowsPos.row(0));
evecsRowsNeg.row(0).copyTo( selEvecsRowsNeg.row(0));
for ( int i = 0; i < 10; ++i)
{
evecsRowsPos.row(i+1).copyTo( selEvecsRowsPos.row(1));
evecsRowsNeg.row(i+1).copyTo( selEvecsRowsNeg.row(1));
std::ostringstream oss;
oss << i+1;
const string rowVals = oss.str();
cv::Mat posProjColVecs = pcaPos.project( selEvecsRowsPos);
const string posfname = string("pos_pts_") + rowVals + string(".txt");
std::ofstream ofs1( posfname.c_str());
RLearning::writePoints( ofs1, (cv::Mat_<double>)posProjColVecs, true);
ofs1.close();
cv::Mat negProjColVecs = pcaNeg.project( selEvecsRowsNeg);
const string negfname = string("neg_pts_") + rowVals + string(".txt");
std::ofstream ofs2( negfname.c_str());
RLearning::writePoints( ofs2, (cv::Mat_<double>)negProjColVecs, true);
ofs2.close();
} // end for
} // end projectToEigenvectors
//*
void ext_pooling(cv::Mat pool_fea, cv::Mat ¢er, cv::Mat &range, int K)
{
int num = pool_fea.rows;
//cv::flann::KDTreeIndexParams indexParams;
//cv::flann::Index fea_tree;
//fea_tree.build(pool_fea, indexParams);
cv::Mat labels;
cv::kmeans(pool_fea, K, labels, cv::TermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 1000, 1e-6), 5, cv::KMEANS_PP_CENTERS, center);
range = cv::Mat::zeros(center.rows, 1, CV_32FC1);
std::vector<size_t> count(center.rows, 0);
int *idx = (int *)labels.data;
for( int i = 0 ; i < pool_fea.rows ; i++, idx++ )
{
float cur_dist = cv::norm(pool_fea.row(i), center.row(*idx), cv::NORM_L2);
//if( range.at<float>(*idx, 0) < cur_dist )
range.at<float>(*idx, 0) += cur_dist;
count[*idx]++;
}
float *ptr = (float *)range.data;
for( int i = 0 ; i < range.rows ; i++, ptr++ )
*ptr /= count[i];
}
void sharpen(const cv::Mat& image, cv::Mat& result)
{
result.create(image.size(), image.type()); // allocate if necessary
for (int j = 1; j < image.rows - 1; j++)
{ // for all rows (except first and last)
const uchar* previous = image.ptr<const uchar>(j - 1); // previous row
const uchar* current = image.ptr<const uchar>(j); // current row
const uchar* next = image.ptr<const uchar>(j + 1); // next row
uchar* output = result.ptr<uchar>(j); // output row
for (int i = 1; i < image.cols - 1; i++)
{
*output++ = cv::saturate_cast<uchar>(5 * current[i] - current[i - 1] - current[i + 1] -
previous[i] - next[i]);
// output[i]=
//cv::saturate_cast<uchar>(5*current[i]-current[i-1]-current[i+1]-previous[i]-next[i]);
}
}
// Set the unprocess pixels to 0
result.row(0).setTo(cv::Scalar(0));
result.row(result.rows - 1).setTo(cv::Scalar(0));
result.col(0).setTo(cv::Scalar(0));
result.col(result.cols - 1).setTo(cv::Scalar(0));
}
void sharpen(const cv::Mat& image, cv::Mat& result)
{
//allocate if necessary
result.create(image.size(),image.type());
for(int j= 1; j < image.rows-1; ++j) { // for all rows
// except the first and last row
const uchar* previous =
image.ptr<uchar>(j-1);
const uchar* current =
image.ptr<uchar>(j);
const uchar* next =
image.ptr<uchar>(j+1);
uchar* output = result.ptr<uchar>(j); // output row
for(int i= 1; i < (image.cols-1) * image.channels(); ++i) {
// stature_cast: avoid the mathematical expression applied on the pixels leads to a
// result that goes out of the range of the permited pixel value( 0 - 255)
*output ++ = cv::saturate_cast<uchar>(5 * current[i] - current[i-1] - current[i+1] -
previous[i] - next[i]);
}
}
//set the unprocess pixels to zero
result.row(0).setTo(cv::Scalar(0));
result.row(result.rows-1).setTo(cv::Scalar(0));
result.col(0).setTo(cv::Scalar(0));
result.col(result.cols-1).setTo(cv::Scalar(0));
}
void sharpen2(const cv::Mat& image, cv::Mat& result)
{
result.create(image.size(), image.type()); // allocate if necessary
int step = image.step1();
const uchar* previous = image.data; // ptr to previous row
const uchar* current = image.data + step; // ptr to current row
const uchar* next = image.data + 2 * step; // ptr to next row
uchar* output = result.data + step; // ptr to output row
for (int j = 1; j < image.rows - 1; j++)
{ // for each row (except first and last)
for (int i = 1; i < image.cols - 1; i++)
{ // for each column (except first and last)
output[i] = cv::saturate_cast<uchar>(5 * current[i] - current[i - 1] - current[i + 1] -
previous[i] - next[i]);
}
previous += step;
current += step;
next += step;
output += step;
}
// Set the unprocess pixels to 0
result.row(0).setTo(cv::Scalar(0));
result.row(result.rows - 1).setTo(cv::Scalar(0));
result.col(0).setTo(cv::Scalar(0));
result.col(result.cols - 1).setTo(cv::Scalar(0));
}
void Processor::sharpen(const cv::Mat &image, cv::Mat &result) {
startTimer();
// allocate if necessary
result.create(image.rows, image.cols, image.type());
for (int j = 1; j < image.rows - 1; j++) { // for all rows
// (except first and last)
const uchar* previous = image.ptr<const uchar>(j - 1); // previous row
const uchar* current = image.ptr<const uchar>(j); // current row
const uchar* next = image.ptr<const uchar>(j + 1); // next row
uchar* output = result.ptr<uchar>(j); // output row
for (int i = 1; i < image.cols - 1; i++) {
for (int k = 0; k < image.channels(); k++) {
result.at<cv::Vec3b>(j, i)[k] = cv::saturate_cast<uchar>(
5 * image.at<cv::Vec3b>(j, i)[k]
- image.at<cv::Vec3b>(j, i - 1)[k]
- image.at<cv::Vec3b>(j, i + 1)[k]
- image.at<cv::Vec3b>(j - 1, i)[k]
- image.at<cv::Vec3b>(j + 1, i)[k]);
}
}
}
// Set the unprocess pixels to 0
result.row(0).setTo(cv::Scalar(0));
result.row(result.rows - 1).setTo(cv::Scalar(0));
result.col(0).setTo(cv::Scalar(0));
result.col(result.cols - 1).setTo(cv::Scalar(0));
stopTimer("Sharpen");
}
// How to do sharpening without explicitly using a convolution filter and cv::filter2D
void RFeatures::sharpen_OLD( const cv::Mat &img, cv::Mat &out)
{
out.create( img.size(), img.type()); // Allocate if necessary
int channels = img.channels();
int nc = img.cols * channels;
for ( int j = 1; j < img.rows-1; ++j) // All rows except first and last
{
const uchar* previous = img.ptr<const uchar>(j-1); // Previous row
const uchar* current = img.ptr<const uchar>(j); // Current row
const uchar* next = img.ptr<const uchar>(j+1); // Next row
uchar* output = out.ptr<uchar>(j); // Output row
for ( int i = channels; i < nc - channels; ++i) // All columns except first and last
{
uchar v = 5*current[i] - current[i-channels] - current[i+channels] - previous[i] - next[i];
*output++ = cv::saturate_cast<uchar>(v);
} // end for
} // end for
// Set the unprocesses pixels to 0
cv::Scalar s(0);
if (img.channels() == 3)
s = cv::Scalar(0,0,0);
out.row(0).setTo( s);
out.row(out.rows-1).setTo( s);
out.col(0).setTo( s);
out.col(out.cols-1).setTo( s);
} // end sharpen_OLD
/*
* Function to convert the training data to be used by the SVM classifier
*/
void convert_to_ml(const std::vector<cv::Mat> &train_samples, cv::Mat& trainData )
{
const int rows = (int)train_samples.size();
const int cols = (int)std::max( train_samples[0].cols, train_samples[0].rows );
cv::Mat tmp(1, cols,CV_32FC1);
trainData = cv::Mat(rows,cols,CV_32FC1 );
std::vector<cv::Mat>::const_iterator itr = train_samples.begin();
std::vector<cv::Mat>::const_iterator end = train_samples.end();
for( int i = 0 ; itr != end ; ++itr, ++i ) {
CV_Assert( itr->cols == 1 || itr->rows == 1 );
if( itr->cols == 1 ) {
transpose( *(itr), tmp );
tmp.copyTo( trainData.row( i ) );
}
else if( itr->rows == 1 ) {
itr->copyTo( trainData.row( i ) );
}
}
}
void sharpen(cv::Mat &image, cv::Mat &out)
{
out.create(image.size(), image.type());
for(int j=1; j < image.rows-1;j++)
{
const uchar* previous = image.ptr<const uchar>(j-1);
const uchar* current = image.ptr<const uchar>(j);
const uchar* next = image.ptr<const uchar>(j+1);
uchar* output = out.ptr<uchar>(j);
for(int i=1; i<image.cols-1; i++)
{
*output++=cv::saturate_cast<uchar>(
5*current[i]-current[i-1]
-current[i+1]-previous[1]-next[1]);
}
}
out.row(0).setTo(cv::Scalar(0));
out.row(out.rows-1).setTo(cv::Scalar(0));
out.col(0).setTo(cv::Scalar(0));
out.col(out.cols-1).setTo(cv::Scalar(0));
}
void _hammingmatch_lowetest_slow(const cv::Mat &_des1, const cv::Mat &_des2, MatchList &_good, const float ratio){
// compute and copmare norms row by row
int _n1 = _des1.rows;
int _n2 = _des2.rows;
int _idx, _mindist1, _mindist2;
cv::DMatch _mtch;
for ( int _i = 0; _i < _n1; ++_i ){
_idx=-1; _mindist1 = 10000; _mindist2 = 1000000; // some arbirtrary large value for initializing
// compute best idx for row _i of _des1
for ( int _j = 0; _j < _n2; ++_j ){
// calculate hamming distance
int _val = cv::normHamming(_des1.row(_i).data, _des2.row(_j).data, 4);
if ( _val > 8) {continue;} // skip to make it fast
_val = cv::normHamming(_des1.row(_i).data, _des2.row(_j).data, _des2.cols);
if ( _val < _mindist2 ){
if ( _val < _mindist1 ){
_mindist2 = _mindist1;
_mindist1 = _val;
_idx = _j;
} else _mindist2 = _val;
}
}
// ratio test
if ( (_idx != -1) && (_mindist1 < (_mindist2 * ratio ))){
// all is okay
printf("distance %d: %d\n", _i, _mindist1);
_mtch.distance = _mindist1;
_mtch.queryIdx = _i; // the first
_mtch.trainIdx = _idx; // the second
_good.push_back(_mtch);
}
}
}
/*
*calculate Affine Matrixes that transform image1 to
*image2 and image2 to image1.
*/
void ImageGraph::calAffineMatrix(ImageNode img1, ImageNode img2, cv::Mat &one2two, cv::Mat &two2one) {
cv::BruteForceMatcher< cv::L2<float> > matcher;
std::vector< cv::DMatch > matches;
matcher.match( img1.descriptors, img2.descriptors, matches );
double max_dist = 0;
for (int i=0; i<matches.size(); i++) {
if (matches[i].distance > max_dist) {
max_dist = matches[i].distance;
}
}
std::vector<cv::DMatch> goodmatches;
std::vector<cv::Point2f> goodpoints1;
std::vector<cv::Point2f> goodpoints2;
for (int i=0; i<matches.size(); i++) {
if (matches[i].distance <= max_dist) {
goodmatches.push_back(matches[i]);
goodpoints1.push_back(img1.keypoints[matches[i].queryIdx].pt);
goodpoints2.push_back(img2.keypoints[matches[i].trainIdx].pt);
}
}
// cv::Mat outp;
// cv::drawMatches(img1.img, img1.keypoints, img2.img, img2.keypoints, goodmatches, outp);
//// cv::namedWindow("aaa");
//cv::imshow("aaa", outp);
// cvWaitKey(0);
cv::Mat t2o = cv::estimateRigidTransform(goodpoints1, goodpoints2, true);
// std::cout << t2o.rows << std::endl;
assert(t2o.rows == 3);
one2two = cv::Mat::zeros(3, 3, CV_64F);
// std::cout << goodpoints1.size() << " " << goodpoints2.size() << std::endl;
// std::cout << "t2o: " << std::endl;
// std::cout << t2o << std::endl;
cv::Mat l = (cv::Mat_<double>(1,3)<< 0,0,1);
t2o.row(0).copyTo(one2two.row(0));
t2o.row(1).copyTo(one2two.row(1));
l.copyTo(one2two.row(2));
cv::Mat o2t = cv::estimateRigidTransform(goodpoints2, goodpoints1, true);
assert(o2t.rows == 3);
two2one = cv::Mat::zeros(3, 3, CV_64F);
o2t.row(0).copyTo(two2one.row(0));
o2t.row(1).copyTo(two2one.row(1));
l.copyTo(two2one.row(2));
std::cout << "two2one:" << std::endl;
std::cout << two2one << std::endl;
std::cout << "one2two" << std::endl;
std::cout << one2two << std::endl;
//std::cout << "asdf " << two2one << std::endl;
// std::cout << "row 0: " << t2o.row(0) << std::endl;
// std::cout << "row 1: " << t2o.row(1) << std::endl;
// std::cout << "row 2: " << l << std::endl;
// std::cout << "xxx " << t2o << std::endl;
// std::cout << "yyy " << one2two << std::endl;
}
void AlphaBlender::performBlendY(const cv::Mat& image1,const cv::Mat& image2,cv::Mat& outputImage){
double alpha=1,beta=0;
for(int i=0;i<image1.rows;i++){
beta=(double)i/(image1.rows-1);
alpha=1-beta;
cv::addWeighted(image1.row(i),alpha,image2.row(i),beta,0,outputImage.row(i));
}
}
inline
cv::Mat
opt_feat::Shift_Image( cv::Mat src_in, int num_pixels_x, int num_pixels_y)
{
cv::Mat img_out;
cv::Mat rot_mat = (cv::Mat_<double>(2,3) << 1, 0, num_pixels_x, 0, 1, num_pixels_y);
warpAffine( src_in, img_out, rot_mat, src_in.size() );
if (num_pixels_x>0) //Move right
{
cv::Mat col = src_in.col(0);
cv::Mat row = src_in.row(0);
for (int i=0; i<abs(num_pixels_x); ++i)
{
col.col(0).copyTo(img_out.col(i));
}
for (int i=0; i<abs(num_pixels_y); ++i)
{
row.row(0).copyTo(img_out.row(i));
//src_in.copyTo(img_out,crop);
}
}
if (num_pixels_x<0) //Move left
{
int w = src_in.size().width;
int h = src_in.size().height;
cv::Mat col = src_in.col(w-1);
cv::Mat row = src_in.row(h-1);
for (int i=w-abs(num_pixels_x) ; i<w; ++i)
{
col.col(0).copyTo(img_out.col(i));
//row.row(0).copyTo(img_out.row(i));
}
for (int i=h-abs(num_pixels_y) ; i<h; ++i)
{
//col.col(0).copyTo(img_out.col(i));
row.row(0).copyTo(img_out.row(i));
}
}
return img_out;
}
void transform (cv::Mat points3d, cv::Mat R, cv::Mat t,
cv::Mat &points3d_out) {
int n = points3d.rows;
points3d_out = Mat (n, 3, matrix_type);
for ( int i = 0; i < n; ++i ) {
// produit de transposé : (R * pt.t() + t.t()).t() == pt * R.t() + t
Mat res = points3d.row(i) * R.t() + t;
res.copyTo(points3d_out.row(i));
}
}
void Writer::writeDistanceMatrix(const std::string &outputFolder_,
const cv::Mat &items_,
const cv::Mat ¢ers_,
const cv::Mat &labels_,
const MetricPtr &metric_)
{
std::vector<std::pair<int, int> > data; // fist=index - second=cluster
for (int i = 0; i < labels_.rows; i++)
{
data.push_back(std::make_pair(i, labels_.at<int>(i)));
}
std::sort(data.begin(), data.end(), comparePairs);
// Generate a matrix of distances between points
float maxDistanceBetween = 0;
cv::Mat distBetweenPoints = cv::Mat::zeros(items_.rows, items_.rows, CV_32FC1);
for (size_t i = 0; i < data.size(); i++)
{
int index1 = data[i].first;
//for (size_t j = 0; j < data.size(); j++)
for (size_t j = 0; j <= i; j++)
{
int index2 = data[j].first;
float distance = (float) metric_->distance(items_.row(index1), items_.row(index2));
distBetweenPoints.at<float>(i, j) = distance;
distBetweenPoints.at<float>(j, i) = distance;
maxDistanceBetween = distance > maxDistanceBetween ? distance : maxDistanceBetween;
}
}
// Generate a matrix of distances to the centers
int pixels = 40;
float maxDistanceToCenter = 0;
cv::Mat distToCenters = cv::Mat::zeros(items_.rows, centers_.rows * pixels, CV_32FC1);
for (size_t i = 0; i < data.size(); i++)
{
int index = data[i].first;
for (int j = 0; j < centers_.rows; j++)
{
float distance = (float) metric_->distance(items_.row(index), centers_.row(j));
distToCenters.row(i).colRange(j * pixels, (j + 1) * pixels) = distance;
maxDistanceToCenter = distance > maxDistanceToCenter ? distance : maxDistanceToCenter;
}
}
// Write images
cv::Mat imageDistBetweenPoints;
distBetweenPoints.convertTo(imageDistBetweenPoints, CV_8UC1, 255 / maxDistanceBetween, 0);
cv::imwrite(outputFolder_ + "distanceBetweenPoints.png", imageDistBetweenPoints);
cv::Mat imageDistToCenter;
distToCenters.convertTo(imageDistToCenter, CV_8UC1, 255 / maxDistanceToCenter, 0);
cv::imwrite(outputFolder_ + "distanceToCenter.png", imageDistToCenter);
}
void MultilayerPerceptron::train_sync(cv::Mat train_data,
cv::Mat expected_outputs)
{
cv::Mat output;
for(int i = 0; i < train_data.rows; ++i)
{
output = feed_forward(train_data.row(i));
backpropagation(expected_outputs.row(i), output);
update_weights();
}
}
void PlotClusters(cv::Mat &dataset, const int *const medoids, const int *const assignment, int nData, int nMedoids)
{
float minx = std::numeric_limits<float>::max();
float miny = std::numeric_limits<float>::max();
float maxx = 0;
float maxy = 0;
for(int i=0; i < dataset.rows; i++)
{
cv::Mat tmp = dataset.row(i);
if(tmp.at<float>(0,0) < minx)
minx = tmp.at<float>(0,0);
if(tmp.at<float>(0,0) > maxx)
maxx = tmp.at<float>(0,0);
if(tmp.at<float>(0,1) < miny)
miny = tmp.at<float>(0,1);
if(tmp.at<float>(0,1) > maxy)
maxy = tmp.at<float>(0,1);
}
float xdim = maxx - minx;
float ydim = maxy - miny;
Eigen::MatrixXd colors(nMedoids,3);
ColorPicker picker(nMedoids);
cv::Mat img = cv::Mat::ones(1024,1024,CV_8UC3);
for(int i=0; i < dataset.rows-1; i++)
{
cv::Mat tmp = dataset.row(i);
float x = ((tmp.at<float>(0,0) - minx)/xdim)*1024;
float y = ((tmp.at<float>(0,1) - miny)/ydim)*1024;
cv::Point2f a(x,y);
Color c = picker.getColor(assignment[i]);
cv::circle(img, a, 2, cv::Scalar(c.r_,c.g_,c.b_), -1 );
}
for(int i=0; i < nMedoids; i++)
{
int center_ind = medoids[i];
cv::Mat tmp = dataset.row(medoids[i]);
float x = ((tmp.at<float>(0,0) - minx)/xdim)*1024;
float y = ((tmp.at<float>(0,1) - miny)/ydim)*1024;
cv::Point2f a(x,y);
Color c = picker.getColor(assignment[medoids[i]]);
cv::circle(img, a, 10, cv::Scalar(c.r_,c.g_,c.b_), -1 );
}
cv::imwrite("Clusters.jpg", img);
// cv::imshow("Clusters",img);
// cv::waitKey(0);
}
//computes dot-product of each column in src with basis vector
inline void dot(const cv::Mat &src, cv::Mat dst) const
{
assert( dst.rows == 1 && dst.cols == src.cols );
assert( dst.type() == src.type() );
for (size_t i = 0; i < positives.size(); i++) {
dst += src.row(positives[i]);
}
for (size_t i = 0; i < negatives.size(); i++) {
dst -= src.row(negatives[i]);
}
}
//*
void refineCenterFixed(cv::Mat center, cv::Mat &new_center, float radius)
{
//cv::Mat buf = cv::norm(range, cv::NORM_L1) / range.rows;
//float radius = buf.at<float>(0, 0);
//std::cerr << "Radius: " << radius << std::endl;
int num = center.rows;
cv::flann::LinearIndexParams indexParams;
cv::flann::Index fea_tree(center, indexParams);
fea_tree.build(center, indexParams);
std::vector< std::pair<int, int> > inds(num);
std::vector< std::vector<int> > neigh_vec(num);
#pragma omp parallel for schedule(dynamic, 1)
for( int i = 0 ; i < num ; i++ )
{
std::vector<float> dists;
std::vector<int> tmp;
int found = fea_tree.radiusSearch(center.row(i), tmp, dists, radius, num, cv::flann::SearchParams());
inds[i].first = i;
inds[i].second = found;
tmp.erase(tmp.begin()+found, tmp.end());
neigh_vec[i] = tmp;
//neigh_vec[i].clear();
//neigh_vec[i].erase(neigh_vec[i].begin()+found, neigh_vec[i].end());
//std::cerr << tmp.size() << std::endl;
//std::cerr << found << std::endl;
//std::cin.get();
//std::cerr << i << std::endl;
}
std::vector<cv::Mat> new_center_vec;
std::vector<bool> flag(num, 0);
std::sort(inds.begin(), inds.end(), comp2);
for( int i = 0 ; i < inds.size() ; i++ )
{
int cur_idx = inds[i].first;
if( flag[cur_idx] == false )
new_center_vec.push_back(center.row(cur_idx));
for( std::vector<int>::iterator p = neigh_vec[cur_idx].begin() ; p < neigh_vec[cur_idx].end() ; p++ )
{
//std::cerr << *p << " ";
flag[*p] = true;
}
//std::cerr << neigh_vec[]
//std::cin.get();
}
cv::vconcat(new_center_vec, new_center);
}
void publishPoint(const std::vector<cv::linemod::Template>& templates, cv::linemod::Match m, cv::Mat& depth, std_msgs::Header header)
{
ROS_DEBUG("Publishing POint");
samplereturn_msgs::NamedPoint img_point_msg;
samplereturn_msgs::NamedPoint point_msg;
img_point_msg.name = m.class_id;
img_point_msg.header = header;
// We only care about the base pyramid level gradient modality
img_point_msg.point.x = m.x + templates[1].width/2;
img_point_msg.point.y = m.y + templates[1].height/2;
ROS_DEBUG("Point x and y: %f, %f", img_point_msg.point.x, img_point_msg.point.y);
LineMOD_Detector::img_point_pub.publish(img_point_msg);
//Check for non-zero
cv::Rect r(m.x,m.y,templates[1].width,templates[1].height);
cv::Mat r_img = depth(r).clone();
r_img.convertTo(r_img, CV_32F);
//char key = (char)cv::waitKey(10);
//cv::imshow("r_img",r_img);
int count = cv::countNonZero(r_img);
if (count > LineMOD_Detector::min_count)
{
//Drop too close or too far
cv::threshold(r_img,r_img,LineMOD_Detector::min_depth, 0, cv::THRESH_TOZERO);
cv::threshold(r_img,r_img,LineMOD_Detector::max_depth, 0, cv::THRESH_TOZERO_INV);
//Grab remaining extrema, compute mean
//The farpoints will probably be ground, the close will be the near side of the object
r_img = r_img.reshape(0,1);
cv::sort(r_img,r_img,CV_SORT_DESCENDING);
double sum = cv::sum(r_img.colRange(0,10))(0) + cv::sum(r_img.colRange(count-10,count))(0);
//The average is the average depth in mm
double ave = sum/20;
//Convert to meters
ave /=1000;
//Now we reproject the center point
cv::Mat image_point = (cv::Mat_<double>(1,3) << m.x,m.y,1);
cv::Mat proj_point = (cv::Mat_<double>(3,1) << 0.,0.,1.);
proj_point.row(0) = inv_K.row(0).dot(image_point);
proj_point.row(1) = inv_K.row(1).dot(image_point);
proj_point.row(2) = inv_K.row(2).dot(image_point);
proj_point *= ave;
point_msg.point.x = proj_point.at<double>(0,0);
point_msg.point.y = proj_point.at<double>(1,0);
point_msg.point.z = proj_point.at<double>(2,0);
std::cout << "Projected Point: " << proj_point << std::endl;
point_msg.name = m.class_id;
point_msg.header = header;
LineMOD_Detector::point_pub.publish(point_msg);
}
}
//===========================================================================
void removeMean(cv::Mat &shape, double &tx, double &ty)
{
cv::Mat avg;
int n = shape.rows;
shape = shape.reshape(0,2);
cv::reduce(shape, avg, 1, CV_REDUCE_AVG);
shape.row(0) = shape.row(0) - avg.rl(0,0);
shape.row(1) = shape.row(1) - avg.rl(1,0);
shape = shape.reshape(0, n);
tx = avg.rl(0,0);
ty = avg.rl(1,0);
}
void Evaluator::computeConfusionMat(cv::Mat &expected, cv::Mat &predicted, cv::Mat &confMat)
{
confMat = cv::Mat::zeros(15,15, CV_32FC1);
std::cout<<expected.rows<<" "<<expected.cols<<std::endl;
for (int i = 0; i < expected.rows; i++) {
confMat.at<float>(expected.at<int>(i,0),predicted.at<int>(i,0))++;
}
// std::cout<<expected.t()<<std::endl;
// std::cout<<predicted.t()<<std::endl;
for (int i = 0; i < confMat.rows; i++)
confMat.row(i) /= sum(confMat.row(i))[0];
}
//assume rows are in the form [y, x]
cv::Mat ModelComponent::cutToSize(cv::Mat x)
{
cv::Mat out;
int height = x.size().height;
for(int c = 0; c < height; c++)
{
int val = x.row(c).at<float>(1);
if(mBegin <= val && val <= mEnd)
{
out.push_back(x.row(c));
}
}
return out;
}
/**
* Computes the feature vector for a set of features
* @param[in] features the list of features used to generate the descriptors
* @param[in] descriptors a list of row-features to create a histogram for
* @param[in] vocab_list a list of row-features to be used as the visual vocabulary
* @return a feature vector
*/
vector<double> bag_of_features::feature_vector(const vector<cv::KeyPoint>
&features, const cv::Mat &descriptors) const {
assert(descriptors.rows == features.size());
// Create an empty histogram
std::vector<double> image_histogram;
image_histogram.resize(vocabulary.centroids.rows * pyramid_size(settings.spatial_pyramid_depth));
std::fill(image_histogram.begin(), image_histogram.end(), 0);
// For each feature, add its contribution to the histogram
for (int feature_num = 0; feature_num < descriptors.rows; feature_num++) {
cv::Mat feature_weight = soft_assign(descriptors.row(feature_num));
std::transform(feature_weight.begin<double>(), // input 1
feature_weight.end<double>(),
image_histogram.begin(), // input 2
image_histogram.begin(), // output
std::plus<double>()); // operator
}
// TODO: Add contribution to each of the spatial histograms
cv::normalize(image_histogram, image_histogram, image_histogram.size(), cv::NORM_L1);
return image_histogram;
}
// ！获取垂直和水平方向直方图
cv::Mat ProjectedHistogram(cv::Mat src, int t){
/*
cv::Mat image1;
IplImage* image2;
image2 = cvCreateImage(cvSize(image1.cols,image1.rows),8,3);
IplImage ipltemp=image1;
cvCopy(&ipltemp,image2);
cv::Mat src = cv::cvarrToMat(img);
//*/
int sz = (t) ? src.rows : src.cols;
cv::Mat mhist = cv::Mat::zeros(1, sz, CV_32F);
for (int j = 0; j<sz; j++){
cv::Mat data = (t) ? src.row(j) : src.col(j);
mhist.at<float>(j) =countNonZero(data); //统计这一行或一列中，非零元素的个数，并保存到mhist中
}
//Normalize histogram
double min, max;
minMaxLoc(mhist, &min, &max);
if (max>0)
mhist.convertTo(mhist, -1, 1.0f / max, 0);//用mhist直方图中的最大值，归一化直方图
//IplImage *dst;
//IplImage tmp = mhist;
//cvCopy(&tmp, dst);
return mhist;
}
void write_table<unsigned char>(ostream& out, cv::Mat& m){
for(int i=0;i<m.rows;++i){
cv::Mat row = m.row(i);
copy(row.begin<unsigned char>(), row.end<unsigned char>(), ostream_iterator<int>(out," "));
out<<endl;
}
}
pcl::CorrespondencesPtr matchSIFT(const cv::Mat &descr1, const cv::Mat &descr2)
{
pcl::CorrespondencesPtr corrs (new pcl::Correspondences());
cv::flann::Index sift_tree;
cv::flann::LinearIndexParams params;
#ifdef opencv_miniflann_build_h
extFlannIndexBuild(sift_tree,descr1, params);
#else
sift_tree.build(descr1, params);
#endif
for( int i = 0 ; i < descr2.rows ; i++ )
{
std::vector<int> idxs;
std::vector<float> dists;
sift_tree.knnSearch(descr2.row(i), idxs, dists, 2, cv::flann::SearchParams());
if( dists[0] / dists[1] < SIFT_RATIO )
{
pcl::Correspondence cur_corr (idxs[0], i, 0);
//std::cerr << idxs[0] << " " << i << std::endl;
corrs->push_back (cur_corr);
}
}
return corrs;
}
