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