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