std::vector<float> CalBarycentric(const std::vector<Point2f> &points, Point2f tf)
{
	Point3f p1 = Point3f(points[0].x, points[0].y, 0.0f);
	Point3f p2 = Point3f(points[1].x, points[1].y, 0.0f);
	Point3f p3 = Point3f(points[2].x, points[2].y, 0.0f);

	Point3f f(tf.x, tf.y, 0.0f);
	Point3f f1 = p1 - f;
	Point3f f2 = p2 - f;
	Point3f f3 = p3 - f;

	Point3f va = (p1 - p2).cross(p1 - p3);
	Point3f va1 = f2.cross(f3);
	Point3f va2 = f3.cross(f1);
	Point3f va3 = f1.cross(f2);

	float a = CalLength(va);
	float a1 = CalLength(va1) / a * sign(va.dot(va1));
	float a2 = CalLength(va2) / a * sign(va.dot(va2));
	float a3 = CalLength(va3) / a * sign(va.dot(va3));

	std::vector<float> w;
	w.push_back(a1);
	w.push_back(a2);
	w.push_back(a3);
	return w;
}
Exemple #2
0
static Point3f findRayIntersection(Point3f k1, Point3f b1, Point3f k2, Point3f b2)
{    
    float a[4], b[2], x[2];
    a[0] = k1.dot(k1);
    a[1] = a[2] = -k1.dot(k2);
    a[3] = k2.dot(k2);
    b[0] = k1.dot(b2 - b1);
    b[1] = k2.dot(b1 - b2);
    Mat_<float> A(2, 2, a), B(2, 1, b), X(2, 1, x);
    solve(A, B, X);
    
    float s1 = X.at<float>(0, 0);
    float s2 = X.at<float>(1, 0);
    return (k1*s1 + b1 + k2*s2 + b2)*0.5f;
};
Exemple #3
0
// Algorithm:
// plane equation: P*N + c = 0
// we find point rays in 3D from image points and camera parameters
// then we fit c by minimizing average L2 distance between rotated and translated object points
// and points found by crossing point rays with plane. We use the fact that center of mass
// of object points and fitted points should coincide.
void findPlanarObjectPose(const vector<Point3f>& _object_points, const Mat& image_points, const Point3f& normal,
                const Mat& intrinsic_matrix, const Mat& distortion_coeffs, double& alpha, double& C, vector<Point3f>& object_points_crf)
{
    vector<Point2f> _rays;
    undistortPoints(image_points, _rays, intrinsic_matrix, distortion_coeffs);

    // filter out rays that are parallel to the plane
    vector<Point3f> rays;
    vector<Point3f> object_points;
    for(size_t i = 0; i < _rays.size(); i++)
    {
        Point3f ray(_rays[i].x, _rays[i].y, 1.0f);
        double proj = ray.dot(normal);
        if(fabs(proj) > std::numeric_limits<double>::epsilon())
        {
            rays.push_back(ray*(1.0/ray.dot(normal)));
            object_points.push_back(_object_points[i]);
        }
    }

    Point3f pc = massCenter(rays);
    Point3f p0c = massCenter(object_points);
    
    vector<Point3f> drays;
    drays.resize(rays.size());
    for(size_t i = 0; i < rays.size(); i++)
    {
        drays[i] = rays[i] - pc;
        object_points[i] -= p0c;
    }

    double s1 = 0.0, s2 = 0.0, s3 = 0.0;
    for(size_t i = 0; i < rays.size(); i++)
    {
        Point3f vprod = crossProduct(drays[i], object_points[i]);
        s1 += vprod.dot(normal);
        s2 += drays[i].dot(object_points[i]);
        s3 += drays[i].dot(drays[i]);
    }
    
    alpha = atan2(s1, s2);
    C = (s2*cos(alpha) + s1*sin(alpha))/s3;
    
//    printf("alpha = %f, C = %f\n", alpha, C);
    
    object_points_crf.resize(rays.size());
    for(size_t i = 0; i < rays.size(); i++)
    {
        object_points_crf[i] = rays[i]*C;
    }
}
Exemple #4
0
// Core Function doing the actual mesh processing.
bool RangeMapPlugin::applyFilter(QAction *filter, MeshDocument &m, FilterParameterSet & par, vcg::CallBackPos *cb)
{
	CMeshO::FaceIterator   fi;

	switch(ID(filter))
  {
		case FP_SELECTBYANGLE : 
		{
			bool usecam = par.getBool("usecamera");
			Point3f viewpoint = par.getPoint3f("viewpoint");	

			// if usecamera but mesh does not have one
			if( usecam && !m.mm()->hasDataMask(MeshModel::MM_CAMERA) ) 
			{
				errorMessage = "Mesh has not a camera that can be used to compute view direction. Please set a view direction."; // text
				return false;
			}
			if(usecam)
			{
				viewpoint = m.mm()->cm.shot.GetViewPoint();
			}

			// angle threshold in radians
			float limit = cos( math::ToRad(par.getDynamicFloat("anglelimit")) );
			Point3f viewray;

			for(fi=m.mm()->cm.face.begin();fi!=m.mm()->cm.face.end();++fi)
				if(!(*fi).IsD())
				{
					viewray = viewpoint - Barycenter(*fi);
					viewray.Normalize();

					if((viewray.dot((*fi).N().Normalize())) < limit)
						fi->SetS();
				}

		}
		break;

	}

	return true;
}
Exemple #5
0
RcppExport SEXP Rclost(SEXP vb_ , SEXP it_, SEXP ioclost_, SEXP sign_, SEXP borderchk_, SEXP barycentric_, SEXP smooth_,SEXP tol_)
{
  try {
    typedef vcg::SpatialHashTable<MyMesh::FaceType, MyMesh::ScalarType> TriMeshGrid; 
    //typedef vcg::GridStaticPtr<MyMesh::FaceType, MyMesh::ScalarType> TriMeshGrid;
    bool signo = as<bool>(sign_);
    bool borderchk = as<bool>(borderchk_);
    bool barycentric = as<bool>(barycentric_);
    bool smooth = as<bool>(smooth_);
    float tol = as<float>(tol_);
    Rcpp::NumericMatrix ioclost(ioclost_);
    int i;
    MyMesh m;
    PcMesh refmesh;
    PcMesh outmesh;
    MyMesh::CoordType baryco;
    // section read from input
    int checkit = Rvcg::IOMesh<MyMesh>::RvcgReadR(m,vb_,it_);
    if (checkit == 1) {
      ::Rf_error("target mesh has no faces, nothing done");
      
    } else if (checkit >= 0) {
      Rvcg::IOMesh<PcMesh>::RvcgReadR(refmesh, ioclost_); 
      m.face.EnableNormal();
 
      tri::UpdateBounding<MyMesh>::Box(m);
      tri::UpdateNormal<MyMesh>::PerFaceNormalized(m);//very important !!!
      //tri::UpdateNormal<MyMesh>::PerVertexAngleWeighted(m);
      tri::UpdateNormal<MyMesh>::PerVertexNormalized(m);
      if (smooth) {
	tri::Smooth<MyMesh>::VertexNormalLaplacian(m,2,false);
	tri::UpdateNormal<MyMesh>::NormalizePerVertex(m);
      }
      float maxDist = m.bbox.Diag()*2;
      if (tol > 0) 
	maxDist = tol;
      float minDist = 1e-10;
      vcg::tri::FaceTmark<MyMesh> mf; 
      mf.SetMesh( &m );
      vcg::face::PointDistanceBaseFunctor<float> PDistFunct;
      TriMeshGrid static_grid;    
      static_grid.Set(m.face.begin(), m.face.end());
      if (borderchk) { //update Border flags
	m.vert.EnableVFAdjacency();
	m.face.EnableFFAdjacency();
	m.face.EnableVFAdjacency();
	tri::UpdateFlags<MyMesh>::FaceBorderFromNone(m);
	tri::UpdateSelection<MyMesh>::FaceFromBorderFlag(m);
      }
      //setup return structure
      Rcpp::NumericMatrix normals(3,refmesh.vn), barycoord(3,refmesh.vn);
      Rcpp::IntegerVector border(refmesh.vn), faceptr(refmesh.vn);
      Rcpp::NumericVector dis(refmesh.vn);
      //index faces
      SimpleTempData<MyMesh::FaceContainer,int> indices(m.face);
      FaceIterator fi=m.face.begin();
      for (i=0; i < m.fn; i++) {
	indices[fi] = i;
	++fi;
      }
      vcg::tri::Append<PcMesh,PcMesh>::Mesh(outmesh,refmesh);
      PcMesh::CoordType tt;
      for(i=0; i < refmesh.vn; i++) {
	border[i] = 0;
	Point3f& currp = refmesh.vert[i].P();
	Point3f& clost = outmesh.vert[i].P();
	MyFace* f_ptr= GridClosest(static_grid, PDistFunct, mf, currp, maxDist, minDist, clost);
	if (f_ptr) {
	  if (borderchk) {
	    if ((*f_ptr).IsS())
	      border[i] = 1;
	  }
	  faceptr[i] = indices[f_ptr];
	  int f_i = vcg::tri::Index(m, f_ptr);
	  tt = currp*0;
	
	  for (int j=0; j <3;j++) {
	    if (&(m.face[f_i].V(j)->N())) {
	      Point3f vdist = m.face[f_i].V(j)->P() - clost;
	      float weight = sqrt(vdist.dot(vdist));
	      if (weight > 0)
		weight = 1/weight;
	      else 
		weight = 1e12;
	    
	      tt +=(m.face[f_i].V(j)->N()*weight);
	    }
	  }
	  if (barycentric) {
	    baryco = currp*0;
	    InterpolationParameters<MyFace,ScalarType>(*f_ptr,f_ptr->N(),clost,baryco);
	  }
	
	  float vl = sqrt(tt.dot(tt));
	  if (vl > 0) {//check for zero length normals
	    tt=tt/vl;
	  
    
	    dis[i] = minDist;
	    if (signo) {
	      Point3f dif = clost - currp;
	      float sign = dif.dot(tt);	
	      if (sign < 0)
		dis[i] = -dis[i] ;
	    }
	  }
	} else {
	  dis[i] = 1e12;
	}
	//write back output
	ioclost(0,i) =clost[0];
	ioclost(1,i) =clost[1];
	ioclost(2,i) =clost[2];
	normals(0,i) = tt[0];
	normals(1,i) = tt[1];    
	normals(2,i) = tt[2];
	if(barycentric) {
	  barycoord(0,i) = baryco[0];
	  barycoord(1,i) = baryco[1];
	  barycoord(2,i) = baryco[2];
	}
      
      }
      return Rcpp::List::create(Rcpp::Named("ioclost") = ioclost,
				Rcpp::Named("barycoord") = barycoord,
				Rcpp::Named("normals") = normals,
				Rcpp::Named("border") = border, 
				Rcpp::Named("distance") = dis,
				Rcpp::Named("faceptr") = faceptr

				);
    } else {
        
      return wrap(1);
    }
  } catch (std::exception& e) {
    ::Rf_error( e.what());
    return wrap(1);
  } catch (...) {
    ::Rf_error("unknown exception");
  }
}
Exemple #6
0
void DomainSurface::makeTri(  Point3f *depth3d , Mat& nbuf,
	Vector4f  intrinRGB)
{
	float T = 0;
	int  near[1000];
	float nearval[1000];
	int ind = 0;

	rline = 0;


	//imshow("label", nbuf);
	//cvWaitKey(-1);

	pairlist.clear();

	vector<float>  list;
//	float *normalData_out = depth->GetData(MEMORYDEVICE_CPU);

	int h = nbuf.rows;
	int w = nbuf.cols;

	Mat buf = Mat(h, w, CV_8SC3);

	buf.setTo(Scalar(-127, -127, -127));


	int ks = 1;


	float vmin = 10000;
	float vmax = -1;
	//find the neighborhood relationship
	for ( int i = 0 ; i < w; i++)
		for (int j = 0; j < h; j++)
		{
			int kc = -1;


			for (int kj = -ks; kj <= ks; kj++)
				for (int ki = -ks; ki <= ks; ki++)
				{
					if (i + ki < 0 || i + ki >= w)
						continue;

					if (j + kj < 0 || j + kj >= h)
						continue;


					int ikx = (kj + j)*w + (i + ki);
					Vec3b c= nbuf.at<Vec3b>(kj + j, i + ki);

					if (kc <= 0)
						kc = c.val[2];

					if (c.val[2] != kc)
					{
						addpair(kc, c.val[2]);
					}

				}


			int idx = ( j)*w + (i  );
			Point3f val = depth3d[idx] ;
			Vec3b belong = nbuf.at<Vec3b>( j, i );
			int cd = belong.val[2];

		 float error =	abs(val.dot(normaldbuf[cd]) + dbuf[cd]);

			//check the error between the belong surface and closest surface

		 int rval, bval, gval;
		 rval = bval = gval = -127;

		 if (error < vmin)
			 vmin = error;
		 if (error > vmax  && error < 100)
		 {
			 vmax = error;
		 }


		 if ( error < 3 )
			 rval =( ((int)(255 * error / 3)) % 255)-127;
		 else if (error < 10)
		 {
			 rval = 127;
			 bval = (((int)(255 * (error-3) / 10)) % 255)-127;

		 }
		 else if ( error < 50)
		 {
			 rval = 127;
			 bval = 127;
			 gval= (((int)(255 * (error-10) / 50)) % 255)-127 ;

		 }


		//	int bval = ((int)(255*error/20  ))%255;

		//291,151

		// if (i == 291 && j == 151)
		//	 cout << "here" << endl;

			buf.at<Vec3b>(j, i) = Vec3b(rval, bval, gval);


		}



	cout << "min error: " << vmin << endl;

	cout << "max error: " << vmax << endl;


	for (int i = 0; i < ndface; i++)
	{

		for each (pair<int,int> var in pairlist)
		{
			int nline = -1;
			if (var.first == i)
			nline=	findLine(i, var.second);
			else if (var.second == i)
			nline=	findLine(i, var.first);

			drawline(buf, nline, intrinRGB);

		}


	//	cv::imshow("", buf);
	//	cvWaitKey(0);

	}