///////////////////////////////////k - means /////////////////////////////////////////////////////////
double cv::ocl::kmeans(const oclMat &_src, int K, oclMat &_bestLabels,
TermCriteria criteria, int attempts, int flags, oclMat &_centers)
{
const int SPP_TRIALS = 3;
bool isrow = _src.rows == 1 && _src.oclchannels() > 1;
int N = !isrow ? _src.rows : _src.cols;
int dims = (!isrow ? _src.cols : 1) * _src.oclchannels();
int type = _src.depth();
attempts = std::max(attempts, 1);
CV_Assert(type == CV_32F && K > 0 );
CV_Assert( N >= K );
Mat _labels;
if( flags & KMEANS_USE_INITIAL_LABELS )
{
CV_Assert( (_bestLabels.cols == 1 || _bestLabels.rows == 1) &&
_bestLabels.cols * _bestLabels.rows == N &&
_bestLabels.type() == CV_32S );
_bestLabels.download(_labels);
}
else
{
if( !((_bestLabels.cols == 1 || _bestLabels.rows == 1) &&
_bestLabels.cols * _bestLabels.rows == N &&
_bestLabels.type() == CV_32S &&
_bestLabels.isContinuous()))
_bestLabels.create(N, 1, CV_32S);
_labels.create(_bestLabels.size(), _bestLabels.type());
}
int* labels = _labels.ptr<int>();
Mat data;
_src.download(data);
Mat centers(K, dims, type), old_centers(K, dims, type), temp(1, dims, type);
std::vector<int> counters(K);
std::vector<Vec2f> _box(dims);
Vec2f* box = &_box[0];
double best_compactness = DBL_MAX, compactness = 0;
RNG& rng = theRNG();
int a, iter, i, j, k;
if( criteria.type & TermCriteria::EPS )
criteria.epsilon = std::max(criteria.epsilon, 0.);
else
criteria.epsilon = FLT_EPSILON;
criteria.epsilon *= criteria.epsilon;
if( criteria.type & TermCriteria::COUNT )
criteria.maxCount = std::min(std::max(criteria.maxCount, 2), 100);
else
criteria.maxCount = 100;
if( K == 1 )
{
attempts = 1;
criteria.maxCount = 2;
}
const float* sample = data.ptr<float>();
for( j = 0; j < dims; j++ )
box[j] = Vec2f(sample[j], sample[j]);
for( i = 1; i < N; i++ )
{
sample = data.ptr<float>(i);
for( j = 0; j < dims; j++ )
{
float v = sample[j];
box[j][0] = std::min(box[j][0], v);
box[j][1] = std::max(box[j][1], v);
}
}
for( a = 0; a < attempts; a++ )
{
double max_center_shift = DBL_MAX;
for( iter = 0;; )
{
swap(centers, old_centers);
if( iter == 0 && (a > 0 || !(flags & KMEANS_USE_INITIAL_LABELS)) )
{
if( flags & KMEANS_PP_CENTERS )
generateCentersPP(data, centers, K, rng, SPP_TRIALS);
else
{
for( k = 0; k < K; k++ )
generateRandomCenter(_box, centers.ptr<float>(k), rng);
}
}
else
{
if( iter == 0 && a == 0 && (flags & KMEANS_USE_INITIAL_LABELS) )
{
for( i = 0; i < N; i++ )
CV_Assert( (unsigned)labels[i] < (unsigned)K );
}
// compute centers
centers = Scalar(0);
for( k = 0; k < K; k++ )
counters[k] = 0;
for( i = 0; i < N; i++ )
{
sample = data.ptr<float>(i);
k = labels[i];
float* center = centers.ptr<float>(k);
j=0;
#if CV_ENABLE_UNROLLED
for(; j <= dims - 4; j += 4 )
{
float t0 = center[j] + sample[j];
float t1 = center[j+1] + sample[j+1];
center[j] = t0;
center[j+1] = t1;
t0 = center[j+2] + sample[j+2];
t1 = center[j+3] + sample[j+3];
center[j+2] = t0;
center[j+3] = t1;
}
#endif
for( ; j < dims; j++ )
center[j] += sample[j];
counters[k]++;
}
if( iter > 0 )
max_center_shift = 0;
for( k = 0; k < K; k++ )
{
if( counters[k] != 0 )
continue;
// if some cluster appeared to be empty then:
// 1. find the biggest cluster
// 2. find the farthest from the center point in the biggest cluster
// 3. exclude the farthest point from the biggest cluster and form a new 1-point cluster.
int max_k = 0;
for( int k1 = 1; k1 < K; k1++ )
{
if( counters[max_k] < counters[k1] )
max_k = k1;
}
double max_dist = 0;
int farthest_i = -1;
float* new_center = centers.ptr<float>(k);
float* old_center = centers.ptr<float>(max_k);
float* _old_center = temp.ptr<float>(); // normalized
float scale = 1.f/counters[max_k];
for( j = 0; j < dims; j++ )
_old_center[j] = old_center[j]*scale;
for( i = 0; i < N; i++ )
{
if( labels[i] != max_k )
continue;
sample = data.ptr<float>(i);
double dist = normL2Sqr_(sample, _old_center, dims);
if( max_dist <= dist )
{
max_dist = dist;
farthest_i = i;
}
}
counters[max_k]--;
counters[k]++;
labels[farthest_i] = k;
sample = data.ptr<float>(farthest_i);
for( j = 0; j < dims; j++ )
{
old_center[j] -= sample[j];
new_center[j] += sample[j];
}
}
for( k = 0; k < K; k++ )
{
float* center = centers.ptr<float>(k);
CV_Assert( counters[k] != 0 );
float scale = 1.f/counters[k];
for( j = 0; j < dims; j++ )
center[j] *= scale;
if( iter > 0 )
{
double dist = 0;
const float* old_center = old_centers.ptr<float>(k);
for( j = 0; j < dims; j++ )
{
double t = center[j] - old_center[j];
dist += t*t;
}
max_center_shift = std::max(max_center_shift, dist);
}
}
}
if( ++iter == MAX(criteria.maxCount, 2) || max_center_shift <= criteria.epsilon )
break;
// assign labels
oclMat _dists(1, N, CV_64F);
_bestLabels.upload(_labels);
_centers.upload(centers);
distanceToCenters(_dists, _bestLabels, _src, _centers);
Mat dists;
_dists.download(dists);
_bestLabels.download(_labels);
double* dist = dists.ptr<double>(0);
compactness = 0;
for( i = 0; i < N; i++ )
{
compactness += dist[i];
}
}
if( compactness < best_compactness )
{
best_compactness = compactness;
}
}
return best_compactness;
}
static void download(const oclMat& ocl_mat, vector<unsigned char>& vec)
{
vec.resize(ocl_mat.cols);
Mat mat(1, ocl_mat.cols, CV_8UC1, (void*)&vec[0]);
ocl_mat.download(mat);
}
void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData)
{
timeLast = timeCur;
timeCur = imageData->header.stamp.toSec() - 0.1163;
cv_bridge::CvImageConstPtr imageDataCv = cv_bridge::toCvShare(imageData, "mono8");
if (!systemInited) {
remap(imageDataCv->image, image0, mapx, mapy, CV_INTER_LINEAR);
oclImage0 = oclMat(image0);
systemInited = true;
return;
}
Mat imageLast, imageCur;
oclMat oclImageLast, oclImageCur;
if (isOddFrame) {
remap(imageDataCv->image, image1, mapx, mapy, CV_INTER_LINEAR);
oclImage1 = oclMat(image1);
imageLast = image0;
imageCur = image1;
oclImageLast = oclImage0;
oclImageCur = oclImage1;
} else {
remap(imageDataCv->image, image0, mapx, mapy, CV_INTER_LINEAR);
oclImage0 = oclMat(image0);
imageLast = image1;
imageCur = image0;
oclImageLast = oclImage1;
oclImageCur = oclImage0;
}
isOddFrame = !isOddFrame;
resize(oclImageLast, oclImageShow, showSize);
oclImageShow.download(imageShow);
cornerHarris(oclImageShow, oclHarrisLast, 2, 3, 0.04);
oclHarrisLast.download(harrisLast);
vector<Point2f> *featuresTemp = featuresLast;
featuresLast = featuresCur;
featuresCur = featuresTemp;
pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast;
imagePointsLast = imagePointsCur;
imagePointsCur = imagePointsTemp;
imagePointsCur->clear();
int recordFeatureNum = totalFeatureNum;
detectionCount = (detectionCount + 1) % (detectionSkipNum + 1);
if (detectionCount == detectionSkipNum) {
oclMat oclFeaturesSub;
for (int i = 0; i < ySubregionNum; i++) {
for (int j = 0; j < xSubregionNum; j++) {
int ind = xSubregionNum * i + j;
int numToFind = maxFeatureNumPerSubregion - subregionFeatureNum[ind];
if (numToFind > maxFeatureNumPerSubregion / 5.0) {
int subregionLeft = xBoundary + (int)(subregionWidth * j);
int subregionTop = yBoundary + (int)(subregionHeight * i);
Rect subregion = Rect(subregionLeft, subregionTop, (int)subregionWidth, (int)subregionHeight);
oclFeatureDetector.maxCorners = numToFind;
oclFeatureDetector(oclImageLast(subregion), oclFeaturesSub);
if (oclFeaturesSub.cols > 0) {
oclFeatureDetector.downloadPoints(oclFeaturesSub, featuresSub);
numToFind = featuresSub.size();
} else {
numToFind = 0;
}
int numFound = 0;
for(int k = 0; k < numToFind; k++) {
featuresSub[k].x += subregionLeft;
featuresSub[k].y += subregionTop;
int xInd = (featuresSub[k].x + 0.5) / showDSRate;
int yInd = (featuresSub[k].y + 0.5) / showDSRate;
if (harrisLast.at<float>(yInd, xInd) > 1e-7) {
featuresLast->push_back(featuresSub[k]);
featuresInd.push_back(featuresIndFromStart);
numFound++;
featuresIndFromStart++;
}
}
totalFeatureNum += numFound;
subregionFeatureNum[ind] += numFound;
}
}
}
}
if (totalFeatureNum > 0) {
Mat featuresLastMat(1, totalFeatureNum, CV_32FC2, (void*)&(*featuresLast)[0]);
oclMat oclFeaturesLast(featuresLastMat);
oclMat oclFeaturesCur, oclFeaturesStatus;
oclOpticalFlow.sparse(oclImageLast, oclImageCur, oclFeaturesLast, oclFeaturesCur, oclFeaturesStatus);
if (oclFeaturesCur.cols > 0 && oclFeaturesCur.cols == oclFeaturesStatus.cols) {
download(oclFeaturesCur, *featuresCur);
download(oclFeaturesStatus, featuresStatus);
totalFeatureNum = featuresCur->size();
} else {
totalFeatureNum = 0;
}
}
for (int i = 0; i < totalSubregionNum; i++) {
subregionFeatureNum[i] = 0;
}
ImagePoint point;
int featureCount = 0;
for (int i = 0; i < totalFeatureNum; i++) {
double trackDis = sqrt(((*featuresLast)[i].x - (*featuresCur)[i].x)
* ((*featuresLast)[i].x - (*featuresCur)[i].x)
+ ((*featuresLast)[i].y - (*featuresCur)[i].y)
* ((*featuresLast)[i].y - (*featuresCur)[i].y));
if (!(trackDis > maxTrackDis || (*featuresCur)[i].x < xBoundary ||
(*featuresCur)[i].x > imageWidth - xBoundary || (*featuresCur)[i].y < yBoundary ||
(*featuresCur)[i].y > imageHeight - yBoundary) && featuresStatus[i]) {
int xInd = (int)(((*featuresLast)[i].x - xBoundary) / subregionWidth);
int yInd = (int)(((*featuresLast)[i].y - yBoundary) / subregionHeight);
int ind = xSubregionNum * yInd + xInd;
if (subregionFeatureNum[ind] < maxFeatureNumPerSubregion) {
(*featuresCur)[featureCount] = (*featuresCur)[i];
(*featuresLast)[featureCount] = (*featuresLast)[i];
featuresInd[featureCount] = featuresInd[i];
point.u = -((*featuresCur)[featureCount].x - kImage[2]) / kImage[0];
point.v = -((*featuresCur)[featureCount].y - kImage[5]) / kImage[4];
point.ind = featuresInd[featureCount];
imagePointsCur->push_back(point);
if (i >= recordFeatureNum) {
point.u = -((*featuresLast)[featureCount].x - kImage[2]) / kImage[0];
point.v = -((*featuresLast)[featureCount].y - kImage[5]) / kImage[4];
imagePointsLast->push_back(point);
}
featureCount++;
subregionFeatureNum[ind]++;
}
}
}
totalFeatureNum = featureCount;
featuresCur->resize(totalFeatureNum);
featuresLast->resize(totalFeatureNum);
featuresInd.resize(totalFeatureNum);
sensor_msgs::PointCloud2 imagePointsLast2;
pcl::toROSMsg(*imagePointsLast, imagePointsLast2);
imagePointsLast2.header.stamp = ros::Time().fromSec(timeLast);
imagePointsLastPubPointer->publish(imagePointsLast2);
showCount = (showCount + 1) % (showSkipNum + 1);
if (showCount == showSkipNum) {
bridge.image = imageShow;
bridge.encoding = "mono8";
sensor_msgs::Image::Ptr imageShowPointer = bridge.toImageMsg();
imageShowPubPointer->publish(imageShowPointer);
}
}
static void download(const oclMat& ocl_mat, vector<Point2f>& vec)
{
vec.resize(ocl_mat.cols);
Mat mat(1, ocl_mat.cols, CV_32FC2, (void*)&vec[0]);
ocl_mat.download(mat);
}
