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