Ejemplo n.º 1
0
int main(int argc, char **argv) 
{
    int k = 1; 
    if (argc == 2) {
        k = atoi(argv[1]);
    }

    //generate data points 
    Point *ps = new Point[N]; 
    for (size_t i = 0; i < N; ++i) { 
        ps[i][0] = 500*(double)rand()/(double)RAND_MAX; 
        ps[i][1] = 500*(double)rand()/(double)RAND_MAX; 
    }

    //generate data points 
    Point *ps2 = new Point[N]; 
    memcpy(ps2, ps, N*sizeof(Point));

    //generate query points 
    Point *qs = new Point[M]; 
    for (size_t i = 0; i < M; ++i) { 
        qs[i] = distfn();
    }

    OddsonTree<Point> oot(2, ps, N, qs, M, MAX_DEPTH); 
    KdTree<Point, double> kdt(2, ps2, N); 

    int errors = 0;

    if (k == 1) {
        for (size_t i = 0; i < Q; ++i) { 
            Point pt = distfn();

            std::list<std::pair<Point *, double> > qr = oot.nn(pt, 0.0); 
            std::list<std::pair<Point *, double> > qr2 = kdt.knn(k, pt, 0.0); 

            if (!std::equal(qr.begin(), qr.end(), qr2.begin(), pred)) {   
                ++errors; 
            } 
        }
    } else { 
        for (size_t i = 0; i < Q; ++i) { 
            Point pt = distfn();

            std::list<std::pair<Point *, double> > qr = oot.knn(k, pt, 0.0); 
            std::list<std::pair<Point *, double> > qr2 = kdt.knn(k, pt, 0.0); 

            if (!std::equal(qr.begin(), qr.end(), qr2.begin(), pred)) {   
                ++errors; 
            } 
        }
    }

    std::cerr << "# of errors: " << errors << " of " << Q << " : "
         << (float)errors/(float)Q*100.0f << " percent.\n";
}
Ejemplo n.º 2
0
 /** estimate the normals of a point cloud */
 static PointCloud<Normal>::Ptr
 compute_pcn(PointCloud<PointXYZ>::ConstPtr in, float vx, float vy, float vz)
 {
   PointCloud<Normal>::Ptr pcn (new PointCloud<Normal>());
   NormalEstimation<PointXYZ, Normal> ne;
   search::KdTree<PointXYZ>::Ptr kdt (new search::KdTree<PointXYZ>());
   ne.setInputCloud(in);
   ne.setSearchMethod(kdt);
   ne.setKSearch(20);
   ne.setViewPoint(vx, vy, vz);
   ne.compute(*pcn);
   return pcn;
 }
Ejemplo n.º 3
0
void NormalEstimator::fitNormal(){
	//
	//compute surface normal
	int ptCnt = m_pt.size();
	if (ptCnt <= 0)
	{
		std::cout << "no point set provided" << std::endl;
		return;
	}
	ANNpointArray ptArray = annAllocPts(ptCnt, 3);
	//assign point values
	for (int i=0; i<ptCnt; i++) {
		cv::Vec3f pt = m_pt[i];
		ANNpoint ptPtr = ptArray[i];
		ptPtr[0] = pt[0];
		ptPtr[1] = pt[1];
		ptPtr[2] = pt[2];
	}

	///
	ANNkd_tree kdt(ptArray, ptCnt, 3);
	
	ANNpoint queryPt = annAllocPt(3);
	int nnCnt = 100;

	ANNidxArray nnIdx = new ANNidx[nnCnt];
	ANNdistArray nnDist = new ANNdist[nnCnt];

	float sigma = -1;
	float evalRatio = 0.05;
	//estimate sigma
	for (int i=0; i < ptCnt; i++) {
		cv::Vec3f pt = m_pt[i];
		queryPt[0] = pt[0];
		queryPt[1] = pt[1];
		queryPt[2] = pt[2];

		//kdt.annkSearch(queryPt,nnCnt, nnIdx, nnDist);
		kdt.annkSearch(queryPt,50, nnIdx, nnDist);
		if (nnDist[49] < sigma ||sigma == -1 )
		{
			sigma = nnDist[49];
		}
	}
	sigma = 0.001;
	std::cout << "search radius:" << sigma << std::endl;
	std::cout << "estimating normals for point set by PCA, be patient..." << std::endl;
	for (int i=0; i < ptCnt; i++) {
		cv::Vec3f pt = m_pt[i];
		queryPt[0] = pt[0];
		queryPt[1] = pt[1];
		queryPt[2] = pt[2];

		//kdt.annkSearch(queryPt,nnCnt, nnIdx, nnDist);
		kdt.annkFRSearch(queryPt, sigma, nnCnt, nnIdx, nnDist);
		int validCnt = 0;
		for (int j = 0; j < nnCnt; j++)
		{
			if (nnIdx[j] == ANN_NULL_IDX)
			{
				break;
			}
			validCnt++;
		}
		//std::cout << validCnt << std::endl;
		if (validCnt < 3)
		{
			continue;
		}
		
		cv::Mat pcaVec(validCnt,3,CV_64FC1);
		cv::Mat pcaMean(1,3,CV_64FC1);
		for (int j = 0; j < validCnt; j++)
		{
			pcaVec.at<double>(j,0) = m_pt[nnIdx[j]][0];
			pcaVec.at<double>(j,1) = m_pt[nnIdx[j]][1];
			pcaVec.at<double>(j,2) = m_pt[nnIdx[j]][2];
		}
		cv::PCA pca(pcaVec,cv::Mat(),CV_PCA_DATA_AS_ROW);

		if (pca.eigenvalues.at<double>(2,0) / pca.eigenvalues.at<double>(1,0) > evalRatio)
		{
			continue;
		}

		m_ptNorm[i] = cv::Vec3f(pca.eigenvectors.at<double>(2,0),pca.eigenvectors.at<double>(2,1),pca.eigenvectors.at<double>(2,2));
		float nr = cv::norm(m_ptNorm[i]);
		m_ptNorm[i][0] /= nr;
		m_ptNorm[i][1] /= nr;
		m_ptNorm[i][2] /= nr;
		//std::cout << m_ptNorm[i][0] << " " << m_ptNorm[i][1] << " " << m_ptNorm[i][2] << std::endl;
		m_ptNormFlag[i] = true;

	}

	//
	std::cout << "done..." << std::endl;
//////////////////////////////////////////////////////////////////////////

	//std::cout << "correct normal direction..." << std::endl;
	//sigma *= 1; //
	//nnCnt *= 1; //
	////reallocate the space for nn idx and nn dist array
	//delete [] nnDist;
	//delete [] nnIdx;
	//nnIdx = new ANNidx[nnCnt];
	//nnDist = new ANNdist[nnCnt];

	//int invertCnt = 0;
	//for (int i = 0; i < ptCnt; i++)
	//{
	//	if (!m_ptNormFlag[i])
	//	{
	//		continue;
	//	}
	//	
	//	cv::Vec3f pt = m_pt[i];
	//	queryPt[0] = pt[0];
	//	queryPt[1] = pt[1];
	//	queryPt[2] = pt[2];

	//	kdt.annkFRSearch(queryPt, sigma, nnCnt, nnIdx, nnDist);
	//	int validCnt = 0, normConsCnt = 0, distConsCnt = 0;
	//	for (int j = 0; j < nnCnt; j++)
	//	{
	//		if (nnIdx[j] == ANN_NULL_IDX)
	//		{
	//			break;
	//		}
	//		else{
	//			//check the direction
	//			cv::Vec3f v1 = m_ptNorm[i];
	//			cv::Vec3f v2 = m_ptNorm[nnIdx[j]];
	//			
	//			if (!m_ptNormFlag[nnIdx[j]])
	//			{
	//				continue;
	//			}else{
	//				//
	//				validCnt++;
	//				if( v2.ddot(v1) > 0 )
	//					normConsCnt++;
	//			}
	//		}
	//	}
	//	//inconsistency detected, invert the direction
	//	if (normConsCnt / validCnt < 0.9)
	//	{
	//		//std::cout << "invert" << std::endl;
	//		invertCnt++;
	//		m_ptNorm[i] = cv::Vec3f(0,0,0) - m_ptNorm[i];
	//	}
	//}
	//std::cout << "# of inverted vertex:" << invertCnt << std::endl;
	////////////////////////////////////////////////////////////////////////////
	
	annDeallocPt(queryPt);
	annDeallocPts(ptArray);
	delete [] nnDist;
	delete [] nnIdx;

}
Ejemplo n.º 4
0
void ANNCluster::doCluster(mat_f& data, grpEle_vect& grpVect, float radius) {
	//
	
	int ptCnt = data.size().height;
	int dim = data.size().width;
	//this is ANN implementation, but it is changed to FLANN now, please refer the updated block below
	float *ptWeight = new float[ptCnt];
	std::vector<bool> mFlag(ptCnt,false);

#ifdef __USE_ANN__
	ANNpointArray ptArray = annAllocPts(ptCnt, dim);
	//assign point values
	for (int i=0; i<ptCnt; i++) {
		ANNpoint ptPtr = ptArray[i];
		for(int j = 0; j < dim; j++){
			float tmp = data(i,j);
			ptPtr[j] = data(i , j);			
		}
	}
	///
	ANNkd_tree kdt(ptArray, ptCnt, dim);
	ANNpoint queryPt = annAllocPt(dim); 
	ANNidxArray nnIdx = new ANNidx[ptCnt];
	ANNdistArray nnDist = new ANNdist[ptCnt];
	int grpCounter = 0;
	for (int i=0; i<ptCnt; i++) {
		if(mFlag[i])
			continue; //skip matched point
		for(int j = 0; j < dim; j++){
			queryPt[j] = data(i,j);
		}
		
		int resultCnt = kdt.annkFRSearch(queryPt, radius, ptCnt, nnIdx, nnDist);
		std::vector<cv::Vec3f> nnPt;
		GrpEle ge;
		ge.m_gID = grpCounter++ ;
		for (int j=0; j < resultCnt; j++) {
			ANNidx idx = nnIdx[j];
			mFlag[idx] = true;
			ANNpoint tmpNNPt = ptArray[idx];
			cv::Vec3f cvPt(tmpNNPt[0], tmpNNPt[1], tmpNNPt[2]);
			ptWeight[j] = exp(-nnDist[j]/radius);
			ge.m_eMember.push_back(Element(idx,ptWeight[j]));
		}
		grpVect.push_back(ge);
	}
	annDeallocPt(queryPt);
	annDeallocPts(ptArray);
	delete [] nnDist;
	delete [] nnIdx;
	delete [] ptWeight;
#else
	cv::flann::KDTreeIndexParams idxParams(4);
	cv::flann::SearchParams scParams(32);
	cv::flann::Index indexer(data,idxParams);
	std::vector<float> nnDists(dim);
	std::vector<int> nnInds(dim);
	std::vector<float> queryPt(dim);
	for(int i = 0; i < ptCnt; i++){
		if(mFlag[i])
			continue;
		for(int j = 0; j < dim; j++)
			queryPt[j] = data(i,j);
		int nnCnt = indexer.radiusSearch(queryPt,nnInds,nnDists,radius,scParams);
		GrpEle ge;
		for(int k = 0; k < nnCnt; k++){
			int tmpIdx = nnInds[k];
			float tmpDist = nnDists[k];
			float tmpWeight = exp(-tmpDist/radius);
			ge.m_eMember.push_back(Element(tmpIdx,tmpWeight));
		}
		grpVect.push_back(ge);
	}
#endif
}
Ejemplo n.º 5
0
void modCalcEquinox::slotCompute()
{
    KStarsData* data = KStarsData::Instance();
    KSSun Sun;
    int year0 = Year->value();

    KStarsDateTime dt( QDate(year0, 1, 1), QTime(0,0,0) );
    long double jd0 = dt.djd(); //save JD on Jan 1st
    for ( int imonth=0; imonth < 12; imonth++ ) {
        KStarsDateTime kdt( QDate(year0, imonth+1, 1), QTime(0,0,0) );
        DMonth[imonth] = kdt.djd() - jd0;
    }

    Plot->removeAllPlotObjects();

    //Add the celestial equator, just a single line bisecting the plot horizontally
    KPlotObject *ce = new KPlotObject( data->colorScheme()->colorNamed( "EqColor" ), KPlotObject::Lines, 2.0 );
    ce->addPoint( 0.0, 0.0 );
    ce->addPoint( 366.0, 0.0 );
    Plot->addPlotObject( ce );

    //Add Ecliptic.  This is more complicated than simply incrementing the
    //ecliptic longitude, because we want the x-axis to be time, not RA.
    //For each day in the year, compute the Sun's position.
    KPlotObject *ecl = new KPlotObject( data->colorScheme()->colorNamed( "EclColor" ), KPlotObject::Lines, 2 );
    ecl->setLinePen( QPen( ecl->pen().color(), 4 ) );

    Plot->setLimits( 1.0, double(dt.date().daysInYear()), -30.0, 30.0 );

    //Add top and bottom axis lines, and custom tickmarks at each month
    addDateAxes();

    for ( int i=1; i<=dt.date().daysInYear(); i++ ) {
        KSNumbers num( dt.djd() );
        Sun.findPosition( &num );
        ecl->addPoint( double(i), Sun.dec().Degrees() );

        dt = dt.addDays( 1 );
    }
    Plot->addPlotObject( ecl );

    dSpring = findEquinox( Year->value(), true, ecl );
    dSummer = findSolstice( Year->value(), true );
    dAutumn = findEquinox( Year->value(), false, ecl );
    dWinter = findSolstice( Year->value(), false );

    //Display the Date/Time of each event in the text fields
    VEquinox->setText( KGlobal::locale()->formatDateTime( dSpring, KLocale::LongDate ) );
    SSolstice->setText( KGlobal::locale()->formatDateTime( dSummer, KLocale::LongDate ) );
    AEquinox->setText( KGlobal::locale()->formatDateTime( dAutumn, KLocale::LongDate ) );
    WSolstice->setText( KGlobal::locale()->formatDateTime( dWinter, KLocale::LongDate ) );

    //Add vertical dotted lines at times of the equinoxes and solstices
    KPlotObject *poSpring = new KPlotObject( Qt::white, KPlotObject::Lines, 1 );
    poSpring->setLinePen( QPen( Qt::white, 1.0, Qt::DotLine ) );
    poSpring->addPoint( dSpring.djd()-jd0, Plot->dataRect().top() );
    poSpring->addPoint( dSpring.djd()-jd0, Plot->dataRect().bottom() );
    Plot->addPlotObject( poSpring );
    KPlotObject *poSummer = new KPlotObject( Qt::white, KPlotObject::Lines, 1 );
    poSummer->setLinePen( QPen( Qt::white, 1.0, Qt::DotLine ) );
    poSummer->addPoint( dSummer.djd()-jd0, Plot->dataRect().top() );
    poSummer->addPoint( dSummer.djd()-jd0, Plot->dataRect().bottom() );
    Plot->addPlotObject( poSummer );
    KPlotObject *poAutumn = new KPlotObject( Qt::white, KPlotObject::Lines, 1 );
    poAutumn->setLinePen( QPen( Qt::white, 1.0, Qt::DotLine ) );
    poAutumn->addPoint( dAutumn.djd()-jd0, Plot->dataRect().top() );
    poAutumn->addPoint( dAutumn.djd()-jd0, Plot->dataRect().bottom() );
    Plot->addPlotObject( poAutumn );
    KPlotObject *poWinter = new KPlotObject( Qt::white, KPlotObject::Lines, 1 );
    poWinter->setLinePen( QPen( Qt::white, 1.0, Qt::DotLine ) );
    poWinter->addPoint( dWinter.djd()-jd0, Plot->dataRect().top() );
    poWinter->addPoint( dWinter.djd()-jd0, Plot->dataRect().bottom() );
    Plot->addPlotObject( poWinter );
}
Ejemplo n.º 6
0
KStarsDateTime KStarsDateTime::addSecs( double s ) const {
    long double ds = (long double)s/86400.;
    KStarsDateTime kdt( djd() + ds );
    return kdt;
}