void determine_features_center() {
if (features_current.size() > 0) {
int clusters = 1;
cv::Mat labels;
cv::Mat centers(clusters, 1, CV_32FC2);
kmeans(cv::Mat(features_current), clusters, labels,
cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 10, 1.0),
3, cv::KMEANS_PP_CENTERS, centers);
std::vector<cv::Point2f> _centers = cv::Mat_ <cv::Point2f>(centers);
smoothing_window.push_back(_centers[0]);
if (smoothing_window.size() > SMOOTHING_WINDOW_SIZE) {
smoothing_window.erase(smoothing_window.begin());
}
int total_x = smoothing_window.at(0).x;
int total_y = smoothing_window.at(0).y;
for (int i=1; i < smoothing_window.size(); i++) {
total_x += smoothing_window.at(i).x;
total_y += smoothing_window.at(i).y;
}
int avg_x = total_x / smoothing_window.size();
int avg_y = total_y / smoothing_window.size();
smooth_features_center.x = avg_x;
smooth_features_center.y = avg_y;
features_center = _centers[0];
} else {
smoothing_window.clear();
}
}
bool TextLocation::getBoundingBox(const IplImage* src, int &clusterNum, CvRect &box) {
vector<CvRect> rects(0);
vector<CvPoint> centers(0);
maxNumLimitedConnectComponet(src, kboxMaxNum, rects, centers);
removeSmallRect(rects, centers);
removePillarRect(rects, centers);
if (centers.size() == 0) {
return false;
} else {
// no clustering
clusterNum = 1;
getUnitedRects(rects, box);
return true;
}
}
double TD_VonMises::getNormalization(const double kappa, const double period) const {
//
std::vector<double> centers(1);
centers[0] = 0.0;
std::vector<double> kappas(1);
kappas[0] = kappa;
std::vector<double> periods(1);
periods[0] = period;
std::vector<double> norm(1);
norm[0] = 1.0;
//
const unsigned int nbins = 1001;
std::vector<double> points;
std::vector<double> weights;
double min = 0.0;
double max = period;
GridIntegrationWeights::getOneDimensionalIntegrationPointsAndWeights(points,weights,nbins,min,max);
//
double sum = 0.0;
for(unsigned int l=0; l<nbins; l++) {
std::vector<double> arg(1); arg[0]= points[l];
sum += weights[l] * VonMisesDiagonal(arg,centers,kappas,periods,norm);
}
return 1.0/sum;
}
template <typename PointNT> void
pcl::MarchingCubesRBF<PointNT>::voxelizeData ()
{
// Initialize data structures
unsigned int N = static_cast<unsigned int> (input_->size ());
Eigen::MatrixXd M (2*N, 2*N),
d (2*N, 1);
for (unsigned int row_i = 0; row_i < 2*N; ++row_i)
{
// boolean variable to determine whether we are in the off_surface domain for the rows
bool row_off = (row_i >= N) ? 1 : 0;
for (unsigned int col_i = 0; col_i < 2*N; ++col_i)
{
// boolean variable to determine whether we are in the off_surface domain for the columns
bool col_off = (col_i >= N) ? 1 : 0;
M (row_i, col_i) = kernel (Eigen::Vector3f (input_->points[col_i%N].getVector3fMap ()).cast<double> () + Eigen::Vector3f (input_->points[col_i%N].getNormalVector3fMap ()).cast<double> () * col_off * off_surface_epsilon_,
Eigen::Vector3f (input_->points[row_i%N].getVector3fMap ()).cast<double> () + Eigen::Vector3f (input_->points[row_i%N].getNormalVector3fMap ()).cast<double> () * row_off * off_surface_epsilon_);
}
d (row_i, 0) = row_off * off_surface_epsilon_;
}
// Solve for the weights
Eigen::MatrixXd w (2*N, 1);
// Solve_linear_system (M, d, w);
w = M.fullPivLu ().solve (d);
std::vector<double> weights (2*N);
std::vector<Eigen::Vector3d> centers (2*N);
for (unsigned int i = 0; i < N; ++i)
{
centers[i] = Eigen::Vector3f (input_->points[i].getVector3fMap ()).cast<double> ();
centers[i + N] = Eigen::Vector3f (input_->points[i].getVector3fMap ()).cast<double> () + Eigen::Vector3f (input_->points[i].getNormalVector3fMap ()).cast<double> () * off_surface_epsilon_;
weights[i] = w (i, 0);
weights[i + N] = w (i + N, 0);
}
for (int x = 0; x < res_x_; ++x)
for (int y = 0; y < res_y_; ++y)
for (int z = 0; z < res_z_; ++z)
{
Eigen::Vector3d point;
point[0] = min_p_[0] + (max_p_[0] - min_p_[0]) * float (x) / float (res_x_);
point[1] = min_p_[1] + (max_p_[1] - min_p_[1]) * float (y) / float (res_y_);
point[2] = min_p_[2] + (max_p_[2] - min_p_[2]) * float (z) / float (res_z_);
double f = 0.0;
std::vector<double>::const_iterator w_it (weights.begin());
for (std::vector<Eigen::Vector3d>::const_iterator c_it = centers.begin ();
c_it != centers.end (); ++c_it, ++w_it)
f += *w_it * kernel (*c_it, point);
grid_[x * res_y_*res_z_ + y * res_z_ + z] = float (f);
}
}
cv::Mat
Auvsi_Recognize::doClustering( cv::Mat input, int numClusters, bool colored = true )
{
#ifdef TWO_CHANNEL
typedef cv::Vec<T, 2> VT;
#else
typedef cv::Vec<T, 3> VT;
#endif
typedef cv::Vec<int, 1> IT;
const int NUMBER_OF_ATTEMPTS = 5;
int inputSize = input.rows*input.cols;
// Create destination image
cv::Mat retVal( input.size(), input.type() );
// Format input to k-means
cv::Mat kMeansData( input );
kMeansData = kMeansData.reshape( input.channels(), inputSize );
// For the output of k-means
cv::Mat labels( inputSize, 1, CV_32S );
cv::Mat centers( numClusters, 1, input.type() );
// Perform the actual k-means clustering
// POSSIBLE FLAGS: KMEANS_PP_CENTERS KMEANS_RANDOM_CENTERS
auto criteria = cv::TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 10, 1.0 );
cv::kmeans( kMeansData, numClusters, labels, criteria , NUMBER_OF_ATTEMPTS, cv::KMEANS_RANDOM_CENTERS, centers );
// Label the image according to the clustering results
cv::MatIterator_<VT> retIterator = retVal.begin<VT>();
cv::MatIterator_<VT> retEnd = retVal.end<VT>();
cv::MatIterator_<IT> labelIterator = labels.begin<IT>();
for( ; retIterator != retEnd; ++retIterator, ++labelIterator )
{
VT data = centers.at<VT>( cv::saturate_cast<int>((*labelIterator)[0]), 0);
#ifdef TWO_CHANNEL
*retIterator = VT( cv::saturate_cast<T>(data[0]), cv::saturate_cast<T>(data[1]) );//, cv::saturate_cast<T>(data[2]) );
#else
*retIterator = VT( cv::saturate_cast<T>(data[0]), cv::saturate_cast<T>(data[1]), cv::saturate_cast<T>(data[2]) );
#endif
}
if( colored )
return retVal;
else
return labels;
}
void drawPick() {
if (!pickedNode) return;
btVector3 p = pickedNode->m_x;
osg::ref_ptr<osg::Vec3Array> centers(new osg::Vec3Array);
centers->push_back(osg::Vec3(p.x(), p.y(), p.z()));
osg::ref_ptr<osg::Vec4Array> cols(new osg::Vec4Array);
cols->push_back(osg::Vec4(1, 0, 0, 0.5));
std::vector<float> radii;
radii.push_back(0.01*METERS);
pickplot->plot(centers, cols, radii);
}
void pcl::PHVObjectClassifier<PointT, PointNormalT, FeatureT>::clusterFeatures(
vector<FeatureT> & cluster_centers, vector<int> & cluster_labels) {
int featureLength = sizeof(features_[0].histogram) / sizeof(float);
cv::Mat feature_vectors = transform_to_mat(features_);
cv::Mat centers(num_clusters_, featureLength, feature_vectors.type()),
labels;
cv::kmeans(feature_vectors, num_clusters_, labels,
cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 10, 1.0), 4,
cv::KMEANS_RANDOM_CENTERS, centers);
transform_to_features(centers, cluster_centers);
cluster_labels = labels;
}
void drawPick() {
osg::ref_ptr<osg::Vec3Array> centers(new osg::Vec3Array);
osg::ref_ptr<osg::Vec4Array> cols(new osg::Vec4Array);
std::vector<float> radii;
if (pickedNode) {
centers->push_back(osg::Vec3(pickedNode->m_x.x(), pickedNode->m_x.y(), pickedNode->m_x.z()));
cols->push_back(osg::Vec4(1, 0, 0, 0.5));
radii.push_back(0.01*METERS);
}
if (pickedNode2) {
centers->push_back(osg::Vec3(pickedNode2->m_x.x(), pickedNode2->m_x.y(), pickedNode2->m_x.z()));
cols->push_back(osg::Vec4(0, 1, 0, 0.5));
radii.push_back(0.01*METERS);
}
pickplot->plot(centers, cols, radii);
}
Arraylist allVariants(int rank, Intstack cell) {
Arraylist ar, result = permVariants(rank, cell) ;
Intstack cen = centers(cell), ce0, ce1;
int i;
for (i = 1; i < cen->size; i++) {
ce0 = newIntstack(0, cell) ;
translate(cen->it[i], ce0) ;
ce1 = somePHDvariant(rank, ce0) ;
if (!memberAr(ce1, result, (int(*)(const void*, const void*)) compareL)) {
#if 0
printf("Variant occurs.\n");
#endif
ar = permVariants(rank, ce1) ;
mergeAr(result, ar, (int(*)(const void*, const void*)) compareL) ;
}
freestack(ce1) ;
}
freestack(cen) ;
return result;
}
void ImportMDEventWorkspace::addEventsData(
typename MDEventWorkspace<MDE, nd>::sptr ws) {
/// Creates a new instance of the MDEventInserter.
MDEventInserter<typename MDEventWorkspace<MDE, nd>::sptr> inserter(ws);
DataCollectionType::iterator mdEventEntriesIterator = m_posMDEventStart;
std::vector<Mantid::coord_t> centers(nd);
for (size_t i = 0; i < m_nDataObjects; ++i) {
float signal = convert<float>(*(++mdEventEntriesIterator));
float error = convert<float>(*(++mdEventEntriesIterator));
uint16_t run_no = 0;
int32_t detector_no = 0;
if (m_IsFullDataObjects) {
run_no = convert<uint16_t>(*(++mdEventEntriesIterator));
detector_no = convert<int32_t>(*(++mdEventEntriesIterator));
}
for (size_t j = 0; j < m_nDimensions; ++j) {
centers[j] = convert<Mantid::coord_t>(*(++mdEventEntriesIterator));
}
// Actually add the mdevent.
inserter.insertMDEvent(signal, error * error, run_no, detector_no,
centers.data());
}
}
Double KMAlgo::ComputeMssc(IPartition const & x, KMInstance const & instance) {
RectMatrix centers(x.maxNbLabels(), instance.nbAtt());
centers.assign(0);
DoubleVector weights(x.maxNbLabels(), 0);
for (auto const & l : x.usedLabels()) {
for (auto const & i : x.observations(l)) {
weights[l] += instance.weight(i);
for (size_t d(0); d < instance.nbAtt(); ++d)
centers.plus(l, d, instance.get(i, d) * instance.weight(i));
}
}
Double result(0);
for (size_t i(0); i < instance.nbObs(); ++i) {
size_t const l(x.label(i));
for (size_t d(0); d < instance.nbAtt(); ++d)
result += instance.weight(i)
* std::pow(
instance.get(i, d) - centers.get(l, d) / weights[l],
2);
}
return result;
}
bool network_generator::network_gen(){
int inlet_dir = -1, outlet_dir = -1, line_layer = 0;
pair <int, int> path_head;
vector < pair <int, int> > inlet(channel_layer*4), outlet(channel_layer*4);
vector <int> inlet_lock(channel_layer*4), outlet_lock(channel_layer*4);
vector < pair <int, int> > centers(4);
centers[0].first = 55;
centers[0].second = 50;
centers[1].first = 50;
centers[1].second = 45;
centers[2].first = 45;
centers[2].second = 50;
centers[3].first = 50;
centers[3].second = 55;
//random_in_out_let(inlet, outlet);
inlet[0].first = 77;//53
inlet[0].second = 100;//0
inlet[1].first = 19;
inlet[1].second = 0;
inlet[2].first = 100;
inlet[2].second = 23;
inlet[3].first = 0;
inlet[3].second = 27;
outlet[0].first = 53;//77
outlet[0].second = 0;//100
outlet[1].first = 100;
outlet[1].second = 35;
outlet[2].first = 0;
outlet[2].second = 65;
outlet[3].first = 49;
outlet[3].second = 100;
for( int i=0;i<inlet.size();i++ ){
pout(inlet[i]);
cout << " ";
pout(outlet[i]);
cout << endl;
}
getchar();
for( int line_num=0;line_num<channel_layer*4;line_num++ ){
vector < vector <double> > temp_heat_map(101, vector <double>(101));
bool reverse = 0;
for( int x=0;x<101;x++ ){
for( int y=0;y<101;y++ ){
double distance_square = pow(x-outlet[line_num].first, 2) + pow(y-outlet[line_num].second, 2);
temp_heat_map[y][x] += 1000 * 10 * (pow(30, 2) / (pow(30, 2) + distance_square));
}
}
path_head = inlet[line_num];
liquid_network[line_num/4][inlet[line_num].second][inlet[line_num].first] = 4;
while(path_head != outlet[line_num]){
//pout(path_head);
//cout <<endl;
//getchar();
if(Is_close_center(path_head, centers[line_num%4]) && reverse == 0){
for( int x=0;x<101;x++ ){
for( int y=0;y<101;y++ ){
if(liquid_network[line_num/4][y][x] == 4){
liquid_network[line_num/4][y][x] = 1;
}
}
}
reverse = 1;
}
if(reverse){
choose_dir(path_head, line_num/4, &temp_heat_map);
liquid_network[line_num/4][path_head.second][path_head.first] = 4;
}
else{
choose_dir(path_head, line_num/4, &init_heat_network[line_num%4]);
liquid_network[line_num/4][path_head.second][path_head.first] = 4;
}
}
for( int x=0;x<101;x++ ){
for( int y=0;y<101;y++ ){
if(liquid_network[line_num/4][y][x] == 4){
liquid_network[line_num/4][y][x] = 1;
}
}
}
//cout << "line done !" << endl;
//print_liquid_network();
//getchar();
}
for( int i=0;i<channel_layer*4;i++ ){
liquid_network[i/4][inlet[i].second][inlet[i].first] = 2;
liquid_network[i/4][outlet[i].second][outlet[i].first] = 3;
}
for( int l=0;l<channel_layer;l++ ){
for( int x=0;x<101;x++ ){
for( int y=0;y<101;y++ ){
if(liquid_network[l][y][x] == 7){
liquid_network[l][y][x] = 0;
}
}
}
}
print_liquid_network();
//getchar();
return true;
}
UnifiedModel* ModelParametersLWPR::toUnifiedModel(void) const
{
if (lwpr_object_->nIn()!=1)
{
//cout << "Warning: Can only call toUnifiedModel() when input dim of LWPR is 1" << endl;
return NULL;
}
if (lwpr_object_->model.nOut!=1)
{
//cout << "Warning: Can only call toUnifiedModel() when output dim of LWPR is 1" << endl;
return NULL;
}
int i_in = 0;
int i_out = 0;
int n_basis_functions = lwpr_object_->model.sub[i_out].numRFS;
VectorXd centers(n_basis_functions);
VectorXd widths(n_basis_functions);
VectorXd offsets(n_basis_functions);
VectorXd slopes(n_basis_functions);
vector<double> tmp;
for (int iRF = 0; iRF < lwpr_object_->model.sub[i_out].numRFS; iRF++)
{
LWPR_ReceptiveFieldObject rf = lwpr_object_->getRF(i_out,iRF);
tmp = rf.center();
centers[iRF] = tmp[i_in];
offsets[iRF] = rf.beta0();
tmp = rf.slope();
slopes[iRF] = tmp[i_in];
tmp = rf.beta();
vector<vector<double> > D = rf.D();
widths[iRF] = sqrt(1.0/D[i_in][i_in]);
}
vector<double> norm_out = lwpr_object_->normOut();
vector<double> norm_in = lwpr_object_->normIn();
//cout << " centers=" << centers.transpose() << endl;
//cout << " widths=" << widths.transpose() << endl;
//cout << " offsets=" << offsets.transpose() << endl;
//cout << " slopes=" << slopes.transpose() << endl;
//cout << " norm_out[i_out]=" << norm_out[i_out] << endl;
//cout << " norm_in[i_in]=" << norm_out[i_in] << endl;
offsets /= norm_out[i_out];
slopes /= norm_out[i_out];
centers /= norm_in[i_in];
widths /= norm_in[i_in];
//cout << " centers=" << centers.transpose() << endl;
//cout << " widths=" << widths.transpose() << endl;
//cout << " offsets=" << offsets.transpose() << endl;
//cout << " slopes=" << slopes.transpose() << endl;
//bool asymmetric_kernels=false;
bool normalized_basis_functions=true;
bool lines_pivot_at_max_activation=true;
return new UnifiedModel(centers,widths,slopes,offsets,
normalized_basis_functions, lines_pivot_at_max_activation);
}
Mat trackVeinsCentres(Mat curvatures, cv::Size imageSize) {
Mat centers(imageSize, curvatures.type());
int wr = 0;
DATA_TYPE scr = 0;
for (auto x = 0; x < imageSize.height; x++) {
for (auto y = 0; y < imageSize.width; y++) {
if (curvatures.at<DATA_TYPE>(x, y, 0) > 0)
wr++;
if (wr > 0 &&
(y == imageSize.width - 1 || (curvatures.at<DATA_TYPE>(x, y, 0) <= 0))) {
int pos_end;
if (y == imageSize.width - 1)
pos_end = y;
else
pos_end = y - 1;
int pos_start = pos_end - wr + 1;
int index = 0;
DATA_TYPE maxValue = std::numeric_limits<DATA_TYPE>::min();
for (auto i = pos_start; i <= pos_end; i++) {
auto currentValue = curvatures.at<DATA_TYPE>(x, i, 0);
if (currentValue > maxValue) {
maxValue = currentValue;
index = i - pos_start;
}
}
int pos_max = pos_start + index - 1;
scr = curvatures.at<DATA_TYPE>(x, pos_max, 0) * wr;
centers.at<DATA_TYPE>(x, pos_max) += scr;
wr = 0;
}
}
}
for (auto y = 0; y < imageSize.width; y++) {
for (auto x = 0; x < imageSize.height; x++) {
if (curvatures.at<DATA_TYPE>(x, y, 1) > 0)
wr++;
if (wr > 0 &&
(x == imageSize.height - 1 || (curvatures.at<DATA_TYPE>(x, y, 1) <= 0))) {
int pos_end;
if (x == imageSize.height - 1)
pos_end = x;
else
pos_end = x - 1;
int pos_start = pos_end - wr + 1;
int index = 0;
DATA_TYPE maxValue = std::numeric_limits<DATA_TYPE>::min();
for (auto i = pos_start; i <= pos_end; i++) {
auto currentValue = curvatures.at<DATA_TYPE>(i, y, 1);
if (currentValue > maxValue) {
maxValue = currentValue;
index = i - pos_start;
}
}
int pos_max = pos_start + index - 1;
scr = curvatures.at<DATA_TYPE>(pos_max, y, 1) * wr;
centers.at<DATA_TYPE>(pos_max, y) += scr;
wr = 0;
}
}
}
for (auto start = 0; start < imageSize.height + imageSize.width - 1; start++) {
int x, y;
if (start < imageSize.width) {
x = start;
y = 0;
} else {
x = 0;
y = start - imageSize.width + 1;
}
while(true) {
if (curvatures.at<DATA_TYPE>(y, x, 2) > 0)
wr++;
if (wr > 0 &&
(y == imageSize.height - 1 || x == imageSize.width - 1 ||
curvatures.at<DATA_TYPE>(y, x, 2) <= 0)) {
int pos_x_end, pos_y_end;
if (y == imageSize.height - 1 || x == imageSize.width - 1) {
pos_x_end = x;
pos_y_end = y;
} else {
pos_x_end = x - 1;
pos_y_end = x - 1;
}
int pos_x_start = pos_x_end - wr + 1;
int pos_y_start = pos_y_end - wr + 1;
int index = 0;
DATA_TYPE maxValue = std::numeric_limits<DATA_TYPE>::min();
for (int i = pos_y_start, j = pos_x_start;
i <= pos_y_end && j <= pos_x_end; i++, j++) {
auto currentValue = curvatures.at<DATA_TYPE>(i, j, 2);
if (currentValue > maxValue) {
maxValue = currentValue;
index = i - pos_y_start;
}
}
int pos_x_max = pos_x_start + index - 1;
int pos_y_max = pos_y_start + index - 1;
scr = curvatures.at<DATA_TYPE>(pos_y_max, pos_x_max, 2) * wr;
centers.at<DATA_TYPE>(pos_y_max, pos_x_max) += scr;
wr = 0;
}
if ((x == imageSize.width - 1) || (y == imageSize.height - 1)) {
break;
} else {
x++;
y++;
}
}
}
for (auto start = 0; start < imageSize.height + imageSize.width - 1; start++) {
int x, y;
if (start < imageSize.width) {
x = start;
y = imageSize.height;
} else {
x = 0;
y = imageSize.width + imageSize.height - start;
}
while(true) {
if (curvatures.at<DATA_TYPE>(y, x, 3) > 0)
wr++;
if (wr > 0 &&
(y == 0 || (x == imageSize.width - 1) ||
(curvatures.at<DATA_TYPE>(y, x, 3) <= 0))) {
int pos_x_end, pos_y_end;
if (y == 0 || x == imageSize.width - 1) {
pos_x_end = x;
pos_y_end = y;
} else {
pos_x_end = x - 1;
pos_y_end = y + 1;
}
int pos_x_start = pos_x_end - wr + 1;
int pos_y_start = pos_y_end + wr - 1;
int index = 0;
DATA_TYPE maxValue = std::numeric_limits<DATA_TYPE>::min();
assert(pos_y_start >= pos_y_end);
assert(pos_x_end >= pos_x_start);
for (int i = pos_y_end, j = pos_x_start;
i <= pos_y_start && j <= pos_x_end; i++, j++) {
auto currentValue = curvatures.at<DATA_TYPE>(i, j, 3);
if (currentValue > maxValue) {
maxValue = currentValue;
index = i - pos_y_end;
}
}
int pos_x_max = pos_x_start + index - 1;
int pos_y_max = pos_y_start - index + 1;
scr = curvatures.at<DATA_TYPE>(pos_y_max, pos_x_max, 3) * wr;
centers.at<DATA_TYPE>(pos_y_max, pos_x_max) += scr;
wr = 0;
}
assert(x < imageSize.width);
assert(y >= 0);
if ((x == imageSize.width - 1) || (y == 0)) {
break;
} else {
x++;
y--;
}
}
}
return centers;
}
int assimpRender(const aiScene* scene, const aiVector3D &robotCenter)
{
std::vector<const aiScene*> scenes(1, scene);
std::vector<aiVector3D> centers(1, robotCenter);
return assimpRender(scenes, centers);
}
// deal with unmatched region
void dealUnMatchedRegion(const Mat& srcImg, const vector<vector<Point2f>>& srcPointsTab, const vector<vector<Point2f>>& srcFeaturesTab, const vector<int>& srcLabels, vector<int>& isMatched, vector<Mat>& transforms )
{
int regionum = transforms.size();
int width = srcImg.size().width;
int height = srcImg.size().height;
vector<Scalar> means(regionum);
computeMeans(srcImg, srcPointsTab, means);
vector<Point2f> centers(regionum);
computeCenters(srcPointsTab, centers);
vector<vector<int>> colorNeighbors(regionum);
buildColorNeighbors(means, colorNeighbors);
//处理未匹配的区域
for (int i = 0; i < regionum; ++ i)
{
if (isMatched[i] != 0) continue;
//集合颜色相近的特征点
int reIdx = i;
vector<Point2f> nearFeatures(0);
for (int j = 0; j < colorNeighbors[i].size(); ++j)
{
int nearIdx = colorNeighbors[i][j];
const vector<Point2f>& nearReFts = srcFeaturesTab[nearIdx];
copy(nearReFts.begin(), nearReFts.end(), std::back_inserter(nearFeatures));
}
//颜色相近的特征点找最近的点
Point2f nearest;
bool isFind = findNearestPoint(nearFeatures, centers[reIdx], nearest);
if (isFind == true)
{
int nearIdx = srcLabels[(int)nearest.y * width + (int)nearest.x];
transforms[reIdx] = transforms[nearIdx].clone();
isMatched[reIdx] = isMatched[nearIdx];
// debug : 显示最近区域
cout << i << "-th unmatched region: the nearest one is " << nearIdx << endl;
Mat test = Mat::zeros(height, width, CV_8UC3);
fillRegion(srcPointsTab[i], means[i], test);
circle(test, centers[i], 5, cv::Scalar(255,255,255));
for (int j = 0; j < colorNeighbors[i].size(); ++ j)
{
int nearIdx = colorNeighbors[i][j];
fillRegion(srcPointsTab[nearIdx], means[nearIdx], test);
}
circle(test, centers[nearIdx], 5, cv::Scalar(255, 255, 255));
string savefn = "output/near_regions_" + type2string(i) + ".png";
imwrite(savefn, test);
//显示结束
}
else
{
cout << i << "-th unmatched region: no similar colored region." << endl;
}
}
}
variant _1_kMeans(std::vector<std::vector<double>> img, std::vector<std::vector<double>> old_centers_v, int K, double epsilon kMeans_DOWN__1_kMeans_decl) {
cv::Mat centers(cv::Scalar(0)), old_centers(old_centers_v);
cv::Mat data0(img);
bool isrow = data0.rows == 1 && data0.channels() > 1;
int N = !isrow ? data0.rows : data0.cols;
int dims = (!isrow ? data0.cols : 1) * data0.channels();
int type = data0.depth();
if (!(data0.dims <= 2 && type == CV_32F && K > 0 && N >= K)) {
error("Cannot perform K-means algorithm for this configuration" kMeans_DOWN__1_kMeans_error_use);
return;
}
cv::Mat data(N, dims, CV_32F, data0.ptr(), isrow ? dims * sizeof(float) : static_cast<size_t>(data0.step));
cv::Mat temp(1, dims, type);
std::vector<int> counters(K, 0);
const float* sample = data.ptr<float>(0);
double max_center_shift = 0;
for (int k = 0; k < K; ++k) {
if (counters[k] != 0)
continue;
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>();
float scale = 1.f/counters[max_k];
for (int j = 0; j < dims; ++j)
_old_center[j] = old_center[j]*scale;
for (int i = 0; i < N; ++i) {
sample = data.ptr<float>(i);
double dist = cv::normL2Sqr_(sample, _old_center, dims);
if (max_dist <= dist) {
max_dist = dist;
farthest_i = i;
}
}
counters[max_k]--;
counters[k]++;
sample = data.ptr<float>(farthest_i);
for (int j = 0; j < dims; ++j) {
old_center[j] -= sample[j];
new_center[j] += sample[j];
}
}
for (int k = 0; k < K; ++k) {
float* center = centers.ptr<float>(k);
if (counters[k] == 0) {
error("For some reason one of the clusters is empty" kMeans_DOWN__1_kMeans_error_use);
return;
}
float scale = 1.f/counters[k];
for (int j = 0; j < dims; ++j)
center[j] *= scale;
double dist = 0;
const float* old_center = old_centers.ptr<float>(k);
for (int j = 0; j < dims; ++j) {
double t = center[j] - old_center[j];
dist += t * t;
}
max_center_shift = std::max(max_center_shift, dist);
}
std::vector<std::vector<double>> _centers;
centers.copyTo(_centers);
if (max_center_shift <= epsilon) {
result(_centers kMeans_DOWN__1_kMeans_result_use);
} else {
_1_loop(img, _centers, K, epsilon kMeans_DOWN__1_kMeans_loop_use);
}
}
void VisionNode::CameraCallback(CCamera *cam, const void *buffer, int buffer_length) {
cv::Mat myuv(HEIGHT + HEIGHT / 2, WIDTH, CV_8UC1, (unsigned char *) buffer);
cv::cvtColor(myuv, img, CV_YUV2RGBA_NV21);
cv::cvtColor(img, img_gray, CV_RGBA2GRAY);
communication::MarkerPosition markerPosition;
markerPosition.header.stamp = ros::Time::now();
static uint next_id = 0;
markerPosition.header.seq = next_id++;
markerPosition.cameraID = ID;
static uint counter = 0;
t2 = std::chrono::high_resolution_clock::now();
time_span = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);
markerPosition.fps = (double)counter/time_span.count();
counter++;
if(time_span.count()>30){ // reset every 30 seconds
counter = 0;
t1 = std::chrono::high_resolution_clock::now();
std_msgs::Int32 msg;
msg.data = ID;
cameraID_pub->publish(msg);
}
cv::Mat filtered_img;
cv::threshold(img_gray, filtered_img, threshold_value, 255, 3);
// find contours in result, which hopefully correspond to a found object
vector <vector<cv::Point>> contours;
vector <cv::Vec4i> hierarchy;
findContours(filtered_img, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE,
cv::Point(0, 0));
// filter out tiny useless contours
double min_contour_area = 10;
for (auto it = contours.begin(); it != contours.end();) {
if (contourArea(*it) < min_contour_area) {
it = contours.erase(it);
}
else {
++it;
}
}
// publish the markerPositions
vector<cv::Point2f> centers(contours.size());
vector<float> radius(contours.size());
for (int idx = 0; idx < contours.size(); idx++) {
minEnclosingCircle(contours[idx], centers[idx], radius[idx]);
communication::Vector2 pos;
pos.x = WIDTH - centers[idx].x;
pos.y = centers[idx].y;
markerPosition.marker_position.push_back(pos);
}
//imshow("camera", img);
//waitKey(1);
markerPosition.markerVisible=contours.size();
marker_position_pub->publish(markerPosition);
if(publish_video_flag && counter%3==0){
// get centers and publish
for (int idx = 0; idx < contours.size(); idx++) {
drawContours(img_gray, contours, idx, cv::Scalar(0, 0, 0), 4, 8, hierarchy, 0,
cv::Point());
}
cv_bridge::CvImage cvImage;
img_gray.copyTo(cvImage.image);
sensor_msgs::Image msg;
cvImage.toImageMsg(msg);
msg.encoding = "mono8";
msg.header = markerPosition.header;
video_pub->publish(msg);
}
}
void Linear_Kernel_Kmeans::InitKmeans(const bool labelsInitialized)
{
if(labelsInitialized==false)
{ // kmeans++ initialization, details see the kmeans++ paper
Array2dC<int> centers(1,k); // index for initial centers
Array2dC<double> portion(1,n);
Array2dC<double> portion_backup(1,n);
std::fill_n(portion.buf,n,DBL_MAX);
int added = 0;
#if defined ( _WIN32 )
unsigned int rr;
rand_s(&rr); while(rr>=n) rand_s(&rr);
#else
int rr;
rr = random(); while(rr>=n) rr=random();
#endif
centers.buf[0] = rr;
std::fill_n(labels,n,0);
added = 1;
while(added<=k)
{
#pragma omp parallel for
for(int i=0;i<n;i++)
{
double v;
if(useMedian==false)
v = x_square[i] + x_square[centers.buf[added-1]] - 2*std::inner_product(data.p[i],data.p[i]+m,data.p[centers.buf[added-1]],0.0);
else
{
v = 0;
for(int j=0;j<m;j++) v += fabs(data.p[i][j]-data.p[centers.buf[added-1]][j]);
}
if(v<portion.buf[i])
{
portion.buf[i] = v;
labels[i] = added - 1;
}
}
if(added<k)
{
for(int i=1;i<n;i++) portion.buf[i] += portion.buf[i-1];
#if defined ( _WIN32 )
if(portion.buf[n-1]<UINT_MAX)
std::copy(portion.buf,portion.buf+n,portion_backup.buf);
else
for(int i=0;i<n;i++) portion_backup.buf[i] = portion.buf[i]/portion.buf[n-1]*UINT_MAX;
rand_s(&rr); while(rr>portion_backup.buf[n-1]) rand_s(&rr);
#else
if(portion.buf[n-1]<RAND_MAX)
std::copy(portion.buf,portion.buf+n,portion_backup.buf);
else
for(int i=0;i<n;i++) portion_backup.buf[i] = portion.buf[i]/portion.buf[n-1]*RAND_MAX;
rr = random(); while(rr>portion.buf[n-1]) rr = random();
#endif
int pos = 0;
while(pos<n && portion_backup.buf[pos]<rr) pos++;
centers.buf[added]=pos;
for(int i=n-1;i>0;i--) portion.buf[i] -= portion.buf[i-1];
}
added++;
}
}
std::fill_n(counts,k,0);
for(int i=0;i<n;i++) counts[labels[i]]++;
}
double cv::kmeans( InputArray _data, int K,
InputOutputArray _bestLabels,
TermCriteria criteria, int attempts,
int flags, OutputArray _centers )
{
const int SPP_TRIALS = 3;
Mat data0 = _data.getMat();
bool isrow = data0.rows == 1;
int N = isrow ? data0.cols : data0.rows;
int dims = (isrow ? 1 : data0.cols)*data0.channels();
int type = data0.depth();
attempts = std::max(attempts, 1);
CV_Assert( data0.dims <= 2 && type == CV_32F && K > 0 );
CV_Assert( N >= K );
Mat data(N, dims, CV_32F, data0.ptr(), isrow ? dims * sizeof(float) : static_cast<size_t>(data0.step));
_bestLabels.create(N, 1, CV_32S, -1, true);
Mat _labels, best_labels = _bestLabels.getMat();
if( flags & CV_KMEANS_USE_INITIAL_LABELS )
{
CV_Assert( (best_labels.cols == 1 || best_labels.rows == 1) &&
best_labels.cols*best_labels.rows == N &&
best_labels.type() == CV_32S &&
best_labels.isContinuous());
best_labels.copyTo(_labels);
}
else
{
if( !((best_labels.cols == 1 || best_labels.rows == 1) &&
best_labels.cols*best_labels.rows == N &&
best_labels.type() == CV_32S &&
best_labels.isContinuous()))
best_labels.create(N, 1, CV_32S);
_labels.create(best_labels.size(), best_labels.type());
}
int* labels = _labels.ptr<int>();
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>(0);
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
Mat dists(1, N, CV_64F);
double* dist = dists.ptr<double>(0);
parallel_for_(Range(0, N),
KMeansDistanceComputer(dist, labels, data, centers));
compactness = 0;
for( i = 0; i < N; i++ )
{
compactness += dist[i];
}
}
if( compactness < best_compactness )
{
best_compactness = compactness;
if( _centers.needed() )
centers.copyTo(_centers);
_labels.copyTo(best_labels);
}
}
return best_compactness;
}
void EnsembleForce::ComputeEnsembleForce(ParticleSubjectArray& shapes) {
if (shapes.size() < 2) {
return;
}
const int nPoints = shapes[0].GetNumberOfPoints();
const int nSubjects = shapes.size();
// Compute offset for origin-centered shapes
VNLMatrix centers(nSubjects, 4);
centers.fill(0);
for (int i = 0; i < nSubjects; i++) {
ParticleSubject& subject = shapes[i];
fordim(k) {
for (int j = 0; j < nPoints; j++) {
Particle& par = subject[j];
centers[i][k] += par.x[k];
}
centers[i][k] /= nPoints;
for (int j = 0; j < nPoints; j++) {
Particle& par = subject[j];
par.x[k] -= centers[i][k];
}
}
}
// Compute xMeanShape
ComputeMeanShape(shapes);
for (int i = 0; i < shapes.size(); i++) {
ParticleBSpline bspline;
bspline.SetReferenceImage(m_ImageContext->GetLabel(i));
bspline.EstimateTransform(shapes[i], m_MeanShape);
FieldTransformType::Pointer fieldTransform = bspline.GetTransform();
shapes[i].TransformX2Y(fieldTransform.GetPointer());
CompositeTransformType::Pointer transform = CompositeTransformType::New();
transform->AddTransform(fieldTransform);
shapes[i].m_Transform = transform;
}
// recover coordinates of particles
for (int i = 0; i < nSubjects; i++) {
ParticleSubject& subject = shapes[i];
fordim (k) {
for (int j = 0; j < nPoints; j++) {
Particle& par = subject[j];
par.x[k] += centers[i][k];
}
}
}
// Compute yMeanShape
fordim(k) {
for (int j = 0; j < nPoints; j++) {
for (int i = 0; i < nSubjects; i++) {
m_MeanShape[j].y[k] += shapes[i][j].y[k];
}
m_MeanShape[j].y[k] /= nSubjects;
}
}
for (int i = 0; i < nSubjects; i++) {
#pragma omp parallel for
for (int j = 0; j < nPoints; j++) {
FieldTransformType::InputPointType xPoint;
FieldTransformType::JacobianType xJac;
xJac.set_size(__Dim,__Dim);
double f[__Dim] = { 0, };
double *x = shapes[i][j].x;
double *y = shapes[i][j].y;
double *my = m_MeanShape[j].y;
fordim(k) {
xPoint[k] = x[k];
}
shapes[i].m_Transform->GetNthTransform(0)->ComputeInverseJacobianWithRespectToPosition(xPoint, xJac);
// shapes[i].m_Transform->ComputeJacobianWithRespectToPosition(xPoint, xJac);
fordim(k) {
if (__Dim == 3) {
f[k] = xJac[0][k]*(y[0]-my[0]) + xJac[1][k]*(y[1]-my[1]) + xJac[2][k]*(y[2]-my[2]);
} else if (__Dim == 2) {
f[k] = xJac[0][k]*(y[0]-my[0]) + xJac[1][k]*(y[1]-my[1]);
}
}
shapes[i][j].SubForce(f, m_Coeff);
}
}
}
std::vector<float> compute_ec_features( USImageType::Pointer input_image, USImageType::Pointer inp_labeled, int number_of_rois, unsigned short thresh, int surr_dist, int inside_dist ){
std::vector< float > qfied_num;
std::vector< USImageType::PixelType > labelsList;
std::vector< double > quantified_numbers_cell;
typedef itk::ExtractImageFilter< USImageType, UShortImageType > LabelExtractType;
typedef itk::ImageRegionConstIterator< UShortImageType > ConstIteratorType;
typedef itk::ImageRegionIteratorWithIndex< USImageType > IteratorType;
typedef itk::SignedDanielssonDistanceMapImageFilter<FloatImageType, FloatImageType > DTFilter;
typedef itk::ImageRegionIteratorWithIndex< FloatImageType > IteratorTypeFloat;
typedef itk::LabelGeometryImageFilter< USImageType > GeometryFilterType;
typedef GeometryFilterType::LabelIndicesType labelindicestype;
itk::SizeValueType sz_x, sz_y, sz_z;
sz_x = input_image->GetLargestPossibleRegion().GetSize()[0];
sz_y = input_image->GetLargestPossibleRegion().GetSize()[1];
sz_z = input_image->GetLargestPossibleRegion().GetSize()[2];
if( sz_x==1 || sz_y==1 || sz_z==1 ){
LabelExtractType::Pointer deFilter = LabelExtractType::New();
USImageType::RegionType dRegion = inp_labeled->GetLargestPossibleRegion();
dRegion.SetSize(2,0);
deFilter->SetExtractionRegion(dRegion);
deFilter->SetInput( inp_labeled );
deFilter->SetDirectionCollapseToIdentity();
try{
deFilter->Update();
}
catch( itk::ExceptionObject & excep ){
std::cerr << "Exception caught !" << std::endl;
std::cerr << excep << std::endl;
}
//Dialate input first
WholeCellSeg *dialate_filter = new WholeCellSeg;
dialate_filter->set_nuc_img( deFilter->GetOutput() );
dialate_filter->set_radius( surr_dist );
dialate_filter->RunSegmentation();
UShortImageType::Pointer input_lab = dialate_filter->getSegPointer();
USImageType::Pointer input_labeled = USImageType::New();
USImageType::PointType origint;
origint[0] = 0;
origint[1] = 0;
origint[2] = 0;
input_labeled->SetOrigin( origint );
USImageType::IndexType startt;
startt[0] = 0; // first index on X
startt[1] = 0; // first index on Y
startt[2] = 0; // first index on Z
USImageType::SizeType sizet;
sizet[0] = inp_labeled->GetLargestPossibleRegion().GetSize()[0]; // size along X
sizet[1] = inp_labeled->GetLargestPossibleRegion().GetSize()[1]; // size along Y
sizet[2] = inp_labeled->GetLargestPossibleRegion().GetSize()[2]; // size along Z
USImageType::RegionType regiont;
regiont.SetSize( sizet );
regiont.SetIndex( startt );
input_labeled->SetRegions( regiont );
input_labeled->Allocate();
input_labeled->FillBuffer(0);
input_labeled->Update();
ConstIteratorType pix_buf1( input_lab, input_lab->GetRequestedRegion() );
IteratorType iterator2 ( input_labeled, input_labeled->GetRequestedRegion() );
iterator2.GoToBegin();
for ( pix_buf1.GoToBegin(); !pix_buf1.IsAtEnd(); ++pix_buf1 ){
iterator2.Set( pix_buf1.Get() );
++iterator2;
}
std::vector< float > quantified_numbers;
typedef itk::LabelGeometryImageFilter< USImageType > GeometryFilterType;
typedef GeometryFilterType::LabelIndicesType labelindicestype;
GeometryFilterType::Pointer geomfilt1 = GeometryFilterType::New();
geomfilt1->SetInput( input_labeled );
geomfilt1->SetCalculatePixelIndices( true );
geomfilt1->Update();
labelsList = geomfilt1->GetLabels();
bool zp=false;
for( USImageType::PixelType i=0; i < labelsList.size(); ++i ){
if( labelsList[i] == 0 ){ zp=true; continue; }
std::vector<float> quantified_numbers_cell;
for( unsigned j=0; j<number_of_rois; ++j ) quantified_numbers_cell.push_back((float)0.0);
double centroid_x = geomfilt1->GetCentroid(labelsList[i])[0];
double centroid_y = geomfilt1->GetCentroid(labelsList[i])[1];
labelindicestype indices1;
indices1 = geomfilt1->GetPixelIndices(labelsList[i]);
for( labelindicestype::iterator itPixind = indices1.begin(); itPixind!=indices1.end(); ++itPixind ){
IteratorType iterator1 ( input_image, input_image->GetRequestedRegion() );
iterator1.SetIndex( *itPixind );
if( iterator1.Get() < thresh )
continue;
double x = iterator1.GetIndex()[0];
double y = iterator1.GetIndex()[1];
double angle = atan2((centroid_y-y),fabs(centroid_x-x));
if( (centroid_x-x)>0 )
angle += M_PI_2;
else
angle = M_PI+M_PI-(angle+M_PI_2);
angle = ((number_of_rois-1)*angle)/(2*M_PI);
double angle_fraction[1];
unsigned angular_index;
if( modf( angle, angle_fraction ) > 0.5 )
angular_index = ceil( angle );
else
angular_index = floor( angle );
quantified_numbers_cell[angular_index] += iterator1.Get();
}
for( unsigned j=0; j<number_of_rois; ++j ) quantified_numbers.push_back(quantified_numbers_cell[j]);
}
unsigned qnum_sz = zp? (labelsList.size()-1) : (labelsList.size());
for( unsigned i=0; i<qnum_sz; ++i ){
unsigned counter=0;
for( unsigned j=0; j<number_of_rois; ++j ){
if( quantified_numbers[(i*number_of_rois+j)] > (255.0*surr_dist) )
++counter;
}
qfied_num.push_back(counter);
}
}
else{
GeometryFilterType::Pointer geomfilt1 = GeometryFilterType::New();
geomfilt1->SetInput( inp_labeled );
geomfilt1->SetCalculatePixelIndices( true );
geomfilt1->Update();
labelsList = geomfilt1->GetLabels();
std::cout<<std::endl<<"The size is: "<<labelsList.size();
if( labelsList.size() == 1 )
{
qfied_num.clear();
return qfied_num;
}
bool zp=false; USPixelType zero;
//Check if the background is also included
for( USPixelType i=0; i<labelsList.size(); ++i ) if( labelsList[i] == 0 ){ zp=true; zero = i; }
USImageType::SizeType sizee;
sizee[0] = inp_labeled->GetLargestPossibleRegion().GetSize()[0]; // size along X
sizee[1] = inp_labeled->GetLargestPossibleRegion().GetSize()[1]; // size along Y
sizee[2] = inp_labeled->GetLargestPossibleRegion().GetSize()[2]; // size along Z
itk::SizeValueType roi_list_size = zp ?
((itk::SizeValueType)number_of_rois*(labelsList.size()-1)*2) :
((itk::SizeValueType)number_of_rois*labelsList.size()*2);
std::vector<double> quantified_numbers_cell((roi_list_size),0.0
);
std::cout<<"Bounding boxes computed"<<std::endl;
#ifdef _OPENMP
itk::MultiThreader::SetGlobalDefaultNumberOfThreads(1);
#pragma omp parallel for
#if _OPENMP < 200805L
for( int i=0; i<labelsList.size(); ++i )
#else
for( USImageType::PixelType i=0; i<labelsList.size(); ++i )
#endif
#else
for( USImageType::PixelType i=0; i<labelsList.size(); ++i )
#endif
{
itk::SizeValueType ind;
if( zp && (zero==i) ) continue;
if( zp && (i>zero) ) ind = i-1;
else ind = i;
//Get label indices
labelindicestype indices1;
double centroid_x,centroid_y,centroid_z;
GeometryFilterType::BoundingBoxType boundbox;
#pragma omp critical
{
indices1 = geomfilt1->GetPixelIndices(labelsList[i]);
//Get Centroid
centroid_x = geomfilt1->GetCentroid(labelsList[i])[0];
centroid_y = geomfilt1->GetCentroid(labelsList[i])[1];
centroid_z = geomfilt1->GetCentroid(labelsList[i])[2];
//Create an image with bounding box + 2 * outside distance + 2
//and get distance map for the label
boundbox = geomfilt1->GetBoundingBox(labelsList[i]);
}
//Create vnl array 3xN( label indicies )
vnl_matrix<double> B(3,indices1.size());
FloatImageType::Pointer inp_lab = FloatImageType::New();
FloatImageType::PointType origint; origint[0] = 0; origint[1] = 0; origint[2] = 0;
inp_lab->SetOrigin( origint );
FloatImageType::IndexType startt;
startt[0] = 0; // first index on X
startt[1] = 0; // first index on Y
startt[2] = 0; // first index on Z
FloatImageType::SizeType sizet;
sizet[0] = boundbox[1]-boundbox[0]+2*surr_dist+2; // size along X
sizet[1] = boundbox[3]-boundbox[2]+2*surr_dist+2; // size along Y
sizet[2] = boundbox[5]-boundbox[4]+2*surr_dist+2; // size along Z
FloatImageType::RegionType regiont;
regiont.SetSize( sizet );
regiont.SetIndex( startt );
inp_lab->SetRegions( regiont );
inp_lab->Allocate();
inp_lab->FillBuffer(0.0);
inp_lab->Update();
IteratorTypeFloat iterator444 ( inp_lab, inp_lab->GetRequestedRegion() );
//Populate matrix with deviations from the centroid for principal axes and
//at the same time set up distance-transform computation
itk::SizeValueType ind1=0;
for( labelindicestype::iterator itPixind = indices1.begin(); itPixind!=indices1.end(); ++itPixind ){
IteratorType iterator3( input_image, input_image->GetRequestedRegion() );
iterator3.SetIndex( *itPixind );
B(0,(ind1)) = iterator3.GetIndex()[0]-centroid_x;
B(1,(ind1)) = iterator3.GetIndex()[1]-centroid_y;
B(2,(ind1)) = iterator3.GetIndex()[2]-centroid_z;
FloatImageType::IndexType cur_in;
cur_in[0] = iterator3.GetIndex()[0]-boundbox[0]+1+surr_dist;
cur_in[1] = iterator3.GetIndex()[1]-boundbox[2]+1+surr_dist;
cur_in[2] = iterator3.GetIndex()[2]-boundbox[4]+1+surr_dist;
iterator444.SetIndex( cur_in );
iterator444.Set( 255.0 );
++ind1;
}
//Compute distance transform for the current object
DTFilter::Pointer dt_obj= DTFilter::New() ;
dt_obj->SetInput( inp_lab );
dt_obj->SquaredDistanceOff();
dt_obj->InsideIsPositiveOff();
try{
dt_obj->Update() ;
} catch( itk::ExceptionObject & err ){
std::cerr << "Error in Distance Transform: " << err << std::endl;
}
FloatImageType::Pointer dist_im = dt_obj->GetOutput();
//Use KLT to compute pricipal axes
vnl_matrix<double> B_transp((int)indices1.size(),3);
B_transp = B.transpose();
vnl_matrix<double> COV(3,3);
COV = B * B_transp;
double norm = 1.0/(double)indices1.size();
COV = COV * norm;
//Eigen decomposition
vnl_real_eigensystem Eyegun( COV );
vnl_matrix<vcl_complex<double> > EVals = Eyegun.D;
double Eval1 = vnl_real(EVals)(0,0); double Eval2 = vnl_real(EVals)(1,1); double Eval3 = vnl_real(EVals)(2,2);
vnl_double_3x3 EVectMat = Eyegun.Vreal;
double V1[3],V2[3],EP_norm[3];
if( Eval1 >= Eval3 && Eval2 >= Eval3 ){
if( Eval1 >= Eval2 ){
V1[0] = EVectMat(0,0); V1[1] = EVectMat(1,0); V1[2] = EVectMat(2,0);
V2[0] = EVectMat(0,1); V2[1] = EVectMat(1,1); V2[2] = EVectMat(2,1);
} else {
V2[0] = EVectMat(0,0); V2[1] = EVectMat(1,0); V2[2] = EVectMat(2,0);
V1[0] = EVectMat(0,1); V1[1] = EVectMat(1,1); V1[2] = EVectMat(2,1);
}
} else if( Eval1 >= Eval2 && Eval3 >= Eval2 ) {
if( Eval1 >= Eval3 ){
V1[0] = EVectMat(0,0); V1[1] = EVectMat(1,0); V1[2] = EVectMat(2,0);
V2[0] = EVectMat(0,2); V2[1] = EVectMat(1,2); V2[2] = EVectMat(2,2);
} else {
V2[0] = EVectMat(0,0); V2[1] = EVectMat(1,0); V2[2] = EVectMat(2,0);
V1[0] = EVectMat(0,2); V1[1] = EVectMat(1,2); V1[2] = EVectMat(2,2);
}
} else {
if( Eval2 >= Eval3 ){
V1[0] = EVectMat(0,1); V1[1] = EVectMat(1,1); V1[2] = EVectMat(2,1);
V2[0] = EVectMat(0,2); V2[1] = EVectMat(1,2); V2[2] = EVectMat(2,2);
} else {
V2[0] = EVectMat(0,1); V2[1] = EVectMat(1,1); V2[2] = EVectMat(2,1);
V1[0] = EVectMat(0,2); V1[1] = EVectMat(1,2); V1[2] = EVectMat(2,2);
}
}
double n_sum = sqrt( V1[0]*V1[0]+V1[1]*V1[1]+V1[2]*V1[2] );
V1[0] /= n_sum; V1[1] /= n_sum; V1[2] /= n_sum;
n_sum = sqrt( V2[0]*V2[0]+V2[1]*V2[1]+V2[2]*V2[2] );
V2[0] /= n_sum; V2[1] /= n_sum; V2[2] /= n_sum;
//Get the normal to the plane formed by the biggest two EVs
EP_norm[0] = V1[1]*V2[2]-V1[2]*V2[1];
EP_norm[1] = V1[2]*V2[0]-V1[0]*V2[2];
EP_norm[2] = V1[0]*V2[1]-V1[1]*V2[0];
//Reassign V2 so that it is orthogonal to both EP_norm and V1
V2[0] = V1[1]*EP_norm[2]-V1[2]*EP_norm[1];
V2[1] = V1[2]*EP_norm[0]-V1[0]*EP_norm[2];
V2[2] = V1[0]*EP_norm[1]-V1[1]*EP_norm[0];
//Now we have the point normal form; EP_norm is the normal and
//centroid_x, centroid_y, centroid_z is the point
//The equation to the plane is EP_norm[0](x-centroid_x)+EP_norm[1](y-centroid_y)+EP_norm[2](z-centroid_z)=0
double dee = (centroid_x*EP_norm[0]+centroid_y*EP_norm[1]+centroid_z*EP_norm[2])*(-1.00);
//Iterate through and assign values to each region
typedef itk::ImageRegionConstIterator< FloatImageType > ConstIteratorTypeFloat;
ConstIteratorTypeFloat pix_buf2( dist_im, dist_im->GetRequestedRegion() );
IteratorType iterator44( input_image, input_image->GetRequestedRegion() );
for ( pix_buf2.GoToBegin(); !pix_buf2.IsAtEnd(); ++pix_buf2 ){
//Use pixels that are only within the defined radius from the nucleus
double current_distance = pix_buf2.Get();
if( (current_distance <= (double)surr_dist) && (current_distance>=(-1*inside_dist)) ){
USImageType::IndexType cur_in;//,cur_in_cpy;
double n_vec[3];
cur_in[0] = pix_buf2.GetIndex()[0]+boundbox[0]-1-surr_dist;
cur_in[1] = pix_buf2.GetIndex()[1]+boundbox[2]-1-surr_dist;
cur_in[2] = pix_buf2.GetIndex()[2]+boundbox[4]-1-surr_dist;
//cur_in_cpy[0] = cur_in[0]; cur_in_cpy[1] = cur_in[1]; cur_in_cpy[2] = cur_in[2];
if( cur_in[0] < 0 || cur_in[1] < 0 || cur_in[2] < 0 ) continue;
if( cur_in[0] >= sizee[0] || cur_in[1] >= sizee[1] || cur_in[2] >= sizee[2] ) continue;
iterator44.SetIndex( cur_in );
USImageType::PixelType pixel_intensity;
pixel_intensity = iterator44.Get();
if( pixel_intensity < thresh ) continue;
//The projection of the point on the plane formed by the fist two major axes
double xxx, yyy, zzz;
xxx = cur_in[0] - EP_norm[0]*((EP_norm[0]*cur_in[0]+EP_norm[1]*cur_in[1]+EP_norm[2]*cur_in[2]+dee)
/(EP_norm[0]*EP_norm[0]+EP_norm[1]*EP_norm[1]+EP_norm[2]*EP_norm[2]));
yyy = cur_in[1] - EP_norm[1]*((EP_norm[0]*cur_in[0]+EP_norm[1]*cur_in[1]+EP_norm[2]*cur_in[2]+dee)
/(EP_norm[0]*EP_norm[0]+EP_norm[1]*EP_norm[1]+EP_norm[2]*EP_norm[2]));
zzz = cur_in[2] - EP_norm[2]*((EP_norm[0]*cur_in[0]+EP_norm[1]*cur_in[1]+EP_norm[2]*cur_in[2]+dee)
/(EP_norm[0]*EP_norm[0]+EP_norm[1]*EP_norm[1]+EP_norm[2]*EP_norm[2]));
//The vector from the centroid to the projected point
n_vec[0] = centroid_x-xxx;
n_vec[1] = centroid_y-yyy;
n_vec[2] = centroid_z-zzz;
n_sum = sqrt( n_vec[0]*n_vec[0] + n_vec[1]*n_vec[1] + n_vec[2]*n_vec[2] );
n_vec[0] /= n_sum; n_vec[1] /= n_sum; n_vec[2] /= n_sum;
//n_vec is the normalized vect in the direction of the projected point
//V1 is the largest eigenvector
//Get the dot and cross product between the two
double doooot, crooos,fin_est_angle;
doooot = n_vec[0]*V1[0]+n_vec[1]*V1[1]+n_vec[2]*V1[2];
crooos = n_vec[0]*V2[0]+n_vec[1]*V2[1]+n_vec[2]*V2[2];
fin_est_angle = atan2( crooos, doooot );
USPixelType bin_num;
//Compute bin num
if( fin_est_angle<0 )
fin_est_angle += (2*M_PI);
bin_num = floor(fin_est_angle*number_of_rois/(2*M_PI));
//Check which side of the plane the point lies on
double v_norm = (cur_in[0]-centroid_x)*(cur_in[0]-centroid_x)
+(cur_in[1]-centroid_y)*(cur_in[1]-centroid_y)
+(cur_in[2]-centroid_z)*(cur_in[2]-centroid_z);
v_norm = sqrt( v_norm );
double doot = (cur_in[0]-centroid_x)*EP_norm[0]/v_norm + (cur_in[1]-centroid_y)*EP_norm[1]/v_norm + (cur_in[2]-centroid_z)*EP_norm[2]/v_norm;
if( doot<0 )
bin_num += number_of_rois;
quantified_numbers_cell.at((ind*(2*number_of_rois)+bin_num)) += pixel_intensity;
}
}
}
number_of_rois = number_of_rois*2;
if( labelsList.size() == 1 )
{
qfied_num.clear();
return qfied_num;
}
std::vector<double> quantified_numbers_cell_cpy(roi_list_size);
std::copy(quantified_numbers_cell.begin(), quantified_numbers_cell.end(), quantified_numbers_cell_cpy.begin() );
//Run k-means
//Most of the code is adapted from mul/mbl/mbl_k_means.cxx
std::sort(quantified_numbers_cell.begin(), quantified_numbers_cell.end());
bool skipKmeans;
double Positive_thresh;
if( quantified_numbers_cell.at(0) == quantified_numbers_cell.at(quantified_numbers_cell.size()-1) )
skipKmeans = true;
if( !skipKmeans ){
std::cout<<"Starting k-means\n";
std::cout<<"First:"<<quantified_numbers_cell.at(0)<<" Last:"<<quantified_numbers_cell.at(quantified_numbers_cell.size()-1)<<std::endl;
unsigned k = 2;
//Vectors and matrices for k-means
std::vector< USImageType::PixelType > partition( roi_list_size, 0 );
std::vector< double > sums ( k, 0.0 );
std::vector< double > centers( k, 0.0 );
std::vector< USImageType::PixelType > nNearest( k, 0 );
//Use the elements that are evenly spaced to get the intial centers
for( unsigned i=0; i<k; ++i ){
double index = ((double)(i)*roi_list_size)/(k+1);
centers.at(i) = quantified_numbers_cell.at((itk::SizeValueType)index);
bool duplicated;
std::cout<<"Initializing centers\n"<<std::flush;
if(i){
if( centers.at((i-1)) == centers.at(i) ){
duplicated = true;
itk::SizeValueType ind=i+1;
while( centers.at((i-1))==quantified_numbers_cell.at(ind) )
++ind;
centers.at(i) = quantified_numbers_cell.at(ind);
sums.at(i) = quantified_numbers_cell.at(ind);
}
}
if(!duplicated)
sums.at(i) = quantified_numbers_cell.at((i+1)/(k+1));
++nNearest[i];
}
bool changed = true;
std::cout<<"Waiting for kmeans to converge\n"<<std::flush;
while(changed){
changed = false;
for(itk::SizeValueType i=0; i<roi_list_size; ++i){
unsigned bestCentre = 0;
double bestDist = fabs((centers.at(0)-quantified_numbers_cell.at(i)));
for(unsigned j=1; j<k; ++j){
double dist = fabs((centers.at(j)-quantified_numbers_cell.at(i)));
if( dist < bestDist ){
bestDist = dist; bestCentre = j;
}
}
sums[bestCentre] += quantified_numbers_cell.at(i);
++ nNearest[bestCentre];
if( bestCentre != partition.at(i) ){
changed = true;
partition.at(i) = bestCentre;
}
}
for( unsigned j=0; j<k; ++j) centers.at(j) = sums.at(j)/nNearest.at(j);
for( unsigned j=0; j<k; ++j) sums.at(j) = 0;
for( unsigned j=0; j<k; ++j) nNearest.at(j) = 0;
}
for( unsigned i=0; i<k; ++i )
std::cout<<"Center "<<i<<" "<<centers.at(i)<<"\n";
Positive_thresh = ((centers.at(0)+centers.at(1))/2) < (255.0*thresh)?
((centers.at(0)+centers.at(1))/2) : (255.0*thresh); //Arbitrary upper thresh
}
else
Positive_thresh = 255.0*thresh;
std::cout<<"Positive_thresh "<<Positive_thresh<<"\n";
std::cout<<"Done k-means\n";
itk::SizeValueType ind = 0;
for( USPixelType i=0; i<labelsList.size(); ++i ){
if( zp && (zero==i) ) continue;
int num_positive_rois = 0;
for( unsigned j=0; j<number_of_rois; ++j ){
itk::SizeValueType index_of_roi = ind*number_of_rois+j;
if( quantified_numbers_cell_cpy.at(index_of_roi)>Positive_thresh )
++num_positive_rois;
}
qfied_num.push_back(num_positive_rois);
++ind;
}
}
std::cout<<"Done surroundedness\n"<<std::flush;
return qfied_num;
}
