Beispiel #1
0
void GrahamScanConvexHull::graham_scan(std::vector< point2d >& final_hull)
{
   const std::size_t HEAD     = 0;
   const std::size_t PRE_HEAD = 1;

   std::deque<gs_point2d> pnt_queue;

   pnt_queue.push_front(point[0]);
   pnt_queue.push_front(point[1]);

   unsigned int i = 2;

   while(i < point.size())
   {
      if (pnt_queue.size() > 1)
      {
         if (orientation(pnt_queue[PRE_HEAD],pnt_queue[HEAD],point[i]) == counter_clock_wise)
            pnt_queue.push_front(point[i++]);
         else
          pnt_queue.pop_front();
      }
      else
         pnt_queue.push_front(point[i++]);
   }

   for(std::deque<gs_point2d>::iterator it = pnt_queue.begin(); it != pnt_queue.end(); it++)
   {
      final_hull.push_back(point2d((*it).x, (*it).y));
   }
}
Beispiel #2
0
	shape makecircle(double radius, point2d centre)
    {
        const point2d xaxis(radius, 0);
        const point2d xaxis_plus (radius, 1);
        const double angleForOnePixel = direction (point2d(0,0), xaxis_plus);
        const int numVertexes = (int)(360.0 / angleForOnePixel);
        shape retVal = make_regular_polygon(radius, numVertexes);
        moveshape(retVal, centre);
        return retVal;
    }
Beispiel #3
0
Ball::Ball(int px, int py, int sx, int sy, int r) {
	/*! \brief Initialize vectors via initialization list
	 */
	x = 0;
	radius = r;
	
	offset = vector2d(px, py);
	slope = vector2d(sx, sy);
	center = point2d(px, py);
}
Beispiel #4
0
// .Explode(ent) - Will create an explosion entity at ent's position
// 		This will not remove ent from the map!
int entity_Explode(lua_State* ls)
{
	Image* img;
	rect r;
	Entity* e = _getReferencedEntity(ls);

	if (e)
	{
		img = e->GetImage();
		if (img)
		{
			r = e->GetBoundingRect();
			new ExplodingEntity(e->mMap, img, point2d(r.x, r.y));
		}
	}

	return 0;
}
Beispiel #5
0
	shape make_regular_polygon(double radius, int n)
    {
        shape retVal;
        double delta = radians(360.0) / n;
        double currentAngle = 0;
        
        while (n--)
        {
            retVal.push_back(point2d(radius * cos(currentAngle), radius * sin(currentAngle)));
            currentAngle += delta;
        }
        
        //now close it.
        if (retVal.size())
        {
            retVal.push_back (retVal.at(0));
        }
        
        return retVal;
    }
Beispiel #6
0
void ActorNameTag::Update()
{
	rect r;
	rect rr;
	
	// reposition self relative to owner
	if (mOwner)
	{
		if (mText != mOwner->mName)
		{	
			mText = mOwner->mName;
		}
	
		r = mOwner->GetBoundingRect();
		rr = GetBoundingRect();
		
		r.y -= rr.h + 2;
		r.x += r.w / 2 - (rr.w / 2);

		SetPosition(point2d(r.x, r.y));
	}
}
	// キャリブレーション
	__declspec(dllexport) void calcCalibration(	double srcPoint2dx[], double srcPoint2dy[], double srcPoint3dx[], double srcPoint3dy[], double srcPoint3dz[], int srcCorrespond, int proWidth, int proHeight, double projectionMatrix[], double externalMatrix[], double& reprojectionResult)
	{
		cv::Mat point2dx(1, srcCorrespond, CV_64F, srcPoint2dx);
		cv::Mat point2dy(1, srcCorrespond, CV_64F, srcPoint2dy);
		cv::Mat point3dx(1, srcCorrespond, CV_64F, srcPoint3dx);
		cv::Mat point3dy(1, srcCorrespond, CV_64F, srcPoint3dy);
		cv::Mat point3dz(1, srcCorrespond, CV_64F, srcPoint3dz);


		// 対応点
		cv::vector<cv::Point2d> imagePoints;
		cv::vector<cv::Point3d> worldPoints;

		for(int i = 0; i < srcCorrespond; ++i)
		{
			cv::Point2d point2d(point2dx.at<double>(i), point2dy.at<double>(i));
			cv::Point3d point3d(point3dx.at<double>(i), point3dy.at<double>(i), point3dz.at<double>(i));

			imagePoints.push_back(point2d);
			worldPoints.push_back(point3d);
		}


		/////////////////// 透視投影変換行列の推定 /////////////////////////////////

		cv::Mat perspectiveMat;		// 透視投影変換行列

		// 透視投影変換行列の推定
		calcProjectionMatrix(worldPoints, imagePoints, perspectiveMat);

		// 再投影誤差
		reprojectionResult = inspection_error_value(perspectiveMat, worldPoints, imagePoints);
		

		perspectiveMat = perspectiveMat/perspectiveMat.at<double>(11);

		cv::Mat cameraMat;
		cv::Mat rotation;
		cv::Mat translate;
		cv::Mat translate2 = cv::Mat::eye(3,1,CV_64F);
		cv::Mat worldTranslate = cv::Mat::eye(3,1,CV_64F);
		cv::decomposeProjectionMatrix(perspectiveMat, cameraMat, rotation, translate);		// 透視投影変換行列の分解→あやしい


		// OpenGL用内部パラメータ

		cameraMat = -cameraMat / cameraMat.at<double>(8);

		double far = 1000.0;
		double near = 0.05;
		
		cv::Mat cameraMatrix0 = cv::Mat::zeros(4, 4, CV_64F);
		cv::Mat cameraMatrix = cv::Mat::zeros(4, 4, CV_64F);


		cameraMatrix0.at<double>(0) = cameraMat.at<double>(0);
		cameraMatrix0.at<double>(1) = cameraMat.at<double>(1);
		cameraMatrix0.at<double>(2) = cameraMat.at<double>(2);
		cameraMatrix0.at<double>(5) = cameraMat.at<double>(4);
		cameraMatrix0.at<double>(6) = cameraMat.at<double>(5);
		cameraMatrix0.at<double>(10) = -(far+near) / (far-near);
		cameraMatrix0.at<double>(11) = -2*far*near / (far-near);
		cameraMatrix0.at<double>(14) = cameraMat.at<double>(8);

		cv::Mat M = cv::Mat::eye(4, 4, CV_64F);
		M.at<double>(0) = 2.0 / (double)proWidth;
		M.at<double>(3) = -1;
		M.at<double>(5) = -2.0 / (double)proHeight;
		M.at<double>(7) = 1;

		cameraMatrix = M * cameraMatrix0;
		cameraMatrix.at<double>(5) = -cameraMatrix.at<double>(5);


		// 外部パラメータ
		translate2.at<double>(0) = translate.at<double>(0)/translate.at<double>(3);
		translate2.at<double>(1) = translate.at<double>(1)/translate.at<double>(3);
		translate2.at<double>(2) = translate.at<double>(2)/translate.at<double>(3);

		wornldTranslate = rotation * translate2; //decomposeProjectionMatrix()のせい?

		//decomposeProjectionMatrix()のせいで-色々つけてる?
		cv::Mat externalMat = cv::Mat::eye(4,4,CV_64F);
		externalMat.at<double>(0) = rotation.at<double>(0);
		externalMat.at<double>(1) = rotation.at<double>(1);
		externalMat.at<double>(2) = rotation.at<double>(2);
		externalMat.at<double>(3) = -worldTranslate.at<double>(0);
		externalMat.at<double>(4) = -rotation.at<double>(3);
		externalMat.at<double>(5) = -rotation.at<double>(4);
		externalMat.at<double>(6) = -rotation.at<double>(5);
		externalMat.at<double>(7) = worldTranslate.at<double>(1);
		externalMat.at<double>(8) = rotation.at<double>(6);
		externalMat.at<double>(9) = rotation.at<double>(7);
		externalMat.at<double>(10) = rotation.at<double>(8);
		externalMat.at<double>(11) = -worldTranslate.at<double>(2);


		// 結果の保存
		cv::FileStorage fs2("Calibration/calibration.xml", cv::FileStorage::WRITE);
		cv::write(fs2,"reprojectionError", reprojectionResult);
		cv::write(fs2,"projectorCalibration", perspectiveMat);
		cv::write(fs2,"internalMatrix", cameraMat);
		cv::write(fs2,"cameraMatrix", cameraMatrix);
		cv::write(fs2,"externalMatrix", externalMat);


		for(int i = 0; i < 16; ++i)
		{
			projectionMatrix[i] = cameraMatrix.at<double>(i);
			externalMatrix[i] = externalMat.at<double>(i);
		}
	}
Beispiel #8
0
void Reconstruction::update (vector<Matcher::p_match> p_matched,Matrix Tr,int32_t point_type,int32_t min_track_length,double max_dist,double min_angle) {
  
  // update transformation vector
  Matrix Tr_total_curr;
  if (Tr_total.size()==0) Tr_total_curr = Matrix::inv(Tr);
  else                    Tr_total_curr = Tr_total.back()*Matrix::inv(Tr);
  Tr_total.push_back(Tr_total_curr);
  Tr_inv_total.push_back(Matrix::inv(Tr_total_curr));
  
  // update projection vector
  Matrix P_total_curr = K*Matrix::inv(Tr_total_curr).getMat(0,0,2,3);
  P_total.push_back(P_total_curr);
  
  // current frame
  int32_t current_frame = Tr_total.size() - 1; // 0-based frame number
  
  // create index vector
  int32_t track_idx_max = 0;
  for (vector<Matcher::p_match>::iterator m=p_matched.begin(); m!=p_matched.end(); m++)
    if (m->i1p > track_idx_max)
      track_idx_max = m->i1p;
  for (vector<track>::iterator t=tracks.begin(); t!=tracks.end(); t++)
    if (t->last_idx > track_idx_max)
      track_idx_max = t->last_idx;
  int32_t *track_idx = new int32_t[track_idx_max+1];
  for (int32_t i=0; i<=track_idx_max; i++)
    track_idx[i] = -1;
  for (int32_t i=0; i<tracks.size(); i++)
    track_idx[tracks[i].last_idx] = i;
  
  // associate matches to tracks
  for (vector<Matcher::p_match>::iterator m=p_matched.begin(); m!=p_matched.end(); m++) {
    
    // track index (-1 = no existing track)
    int32_t idx = track_idx[m->i1p];
    
    // add to existing track
    if (idx>=0 && tracks[idx].last_frame==current_frame-1) {
      
      tracks[idx].pixels.push_back(point2d(m->u1c,m->v1c));
      tracks[idx].last_frame = current_frame;
      tracks[idx].last_idx   = m->i1c;
      
    // create new track
    } else {
      track t;
      t.pixels.push_back(point2d(m->u1p,m->v1p));
      t.pixels.push_back(point2d(m->u1c,m->v1c));
      t.first_frame = current_frame-1;
      t.last_frame  = current_frame;
      t.last_idx    = m->i1c;
      tracks.push_back(t);
    }
  }
  
  // copy tracks
  vector<track> tracks_copy = tracks;
  tracks.clear();
  
  // devise tracks into active or reconstruct 3d points
  for (vector<track>::iterator t=tracks_copy.begin(); t!=tracks_copy.end(); t++) {
    
    // track has been extended
    if (t->last_frame==current_frame) {
      
      // push back to active tracks
      tracks.push_back(*t);
      
    // track lost
    } else {
      
      // add to 3d reconstruction
      if (t->pixels.size()>=min_track_length) {
        
        // 3d point
        point3d p;
        
        // try to init point from first and last track frame
        if (initPoint(*t,p)) {
          if (pointType(*t,p)>=point_type)
            if (refinePoint(*t,p))
              if(pointDistance(*t,p)<max_dist && rayAngle(*t,p)>min_angle)
                points.push_back(p);
        }
      }
    }
  }
  
  //cout << "P: " << points.size() << endl;
  //testJacobian();
  
  delete track_idx;
}
Beispiel #9
0
	point2d shift_linear(float _x,float _y)        {return point2d(x+_x,y+_y);};
Beispiel #10
0
	point2d shift_radial(float radius,float angle) {return point2d(x+cos(angle)*radius,y+sin(angle)*radius);};