void MxPropSlim::discontinuity_constraint(MxVertexID i, MxVertexID j, MxFaceID f)
{
Vec3 org(m->vertex(i)), dest(m->vertex(j));
Vec3 e = dest - org;
Vec3 v1(m->vertex(m->face(f)(0)));
Vec3 v2(m->vertex(m->face(f)(1)));
Vec3 v3(m->vertex(m->face(f)(2)));
Vec3 n = triangle_normal(v1,v2,v3);
Vec3 n2 = e ^ n;
unitize(n2);
MxQuadric3 Q3(n2, -(n2*org));
Q3 *= boundary_weight;
if( weighting_policy == MX_WEIGHT_AREA ||
weighting_policy == MX_WEIGHT_AREA_AVG )
{
Q3.set_area(norm2(e));
Q3 *= Q3.area();
}
MxQuadric Q(Q3, dim());
quadric(i) += Q;
quadric(j) += Q;
}
void MxPropSlim::collect_quadrics()
{
for(unsigned int j=0; j<quadric_count(); j++)
__quadrics[j] = xr_new<MxQuadric>(dim());
for(MxFaceID i=0; i<m->face_count(); i++)
{
MxFace& f = m->face(i);
MxQuadric Q (dim());
compute_face_quadric(i, Q);
switch( weighting_policy )
{
case MX_WEIGHT_AREA:
case MX_WEIGHT_AREA_AVG:
Q *= Q.area();
// no break: fallthrough
default:
quadric(f[0]) += Q;
quadric(f[1]) += Q;
quadric(f[2]) += Q;
break;
}
}
}
void MxPropSlim::apply_contraction(const MxPairContraction& conx,
edge_info *info)
{
valid_verts--;
valid_faces -= conx.dead_faces.length();
quadric(conx.v1) += quadric(conx.v2);
update_pre_contract(conx);
m->apply_contraction(conx);
unpack_from_vector(conx.v1, info->target);
// Must update edge_info here so that the meshing penalties
// will be computed with respect to the new mesh rather than the old
for(unsigned int i=0; i<(unsigned int)edge_links(conx.v1).length(); i++)
compute_edge_info(edge_links(conx.v1)[i]);
}
void MxPropSlim::compute_target_placement(edge_info *info)
{
MxVertexID i=info->v1, j=info->v2;
const MxQuadric &Qi=quadric(i), &Qj=quadric(j);
MxQuadric Q=Qi; Q+=Qj;
double e_min;
if( placement_policy==MX_PLACE_OPTIMAL && Q.optimize(info->target)){
e_min = Q(info->target);
}else{
// Fall back only on endpoints
MxVector vi(dim());
MxVector vj(dim());
MxVector best(dim());
pack_to_vector(i, vi);
pack_to_vector(j, vj);
double ei=Q(vi), ej=Q(vj);
if( ei<ej ) { e_min = ei; best = vi; }
else { e_min = ej; best = vj; swap(info->v1,info->v2);}
if( placement_policy>=MX_PLACE_ENDORMID ){
MxVector mid(dim());
mid = vi;
mid +=vj;
mid /=2.f;
double e_mid= Q(mid);
if( e_mid < e_min ) { e_min = e_mid; best = mid; }
}
info->target = best;
}
if( weighting_policy == MX_WEIGHT_AREA_AVG )
e_min /= Q.area();
info->heap_key(float(-e_min));
}
struct shape *paraboloid(const struct coord *base, const struct coord *apex,
double r)
{
return quadric(base, apex, r, paraboloid_intersect, normal);
}
int main(int argc, char** argv)
{
double taubin_radius = 0.03; // radius of curvature-estimation neighborhood
double hand_radius = 0.08; // radius of hand configuration neighborhood
// std::string file = "/home/andreas/data/mlaffordance/round21l_reg.pcd";
std::string file = "/home/andreas/data/mlaffordance/training/rect31l_reg.pcd";
PointCloud::Ptr cloud(new PointCloud);
if (pcl::io::loadPCDFile<pcl::PointXYZ>(file, *cloud) == -1) //* load the file
{
PCL_ERROR("Couldn't read input PCD file\n");
return (-1);
}
pcl::search::OrganizedNeighbor<pcl::PointXYZ>::Ptr organized_neighbor(
new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
std::vector<int> nn_indices;
std::vector<float> nn_dists;
pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZ>());
// int sample_index = 0;
int sample_index = 731;
if (cloud->isOrganized())
{
organized_neighbor->setInputCloud(cloud);
organized_neighbor->radiusSearch(cloud->points[sample_index], taubin_radius, nn_indices, nn_dists);
}
else
{
tree->setInputCloud(cloud);
tree->radiusSearch(cloud->points[sample_index], taubin_radius, nn_indices, nn_dists);
}
std::cout << "Found point neighborhood for sample " << sample_index << "\n";
Eigen::Matrix4d T_base, T_sqrt;
T_base << 0, 0.445417, 0.895323, 0.22, 1, 0, 0, -0.02, 0, 0.895323, -0.445417, 0.24, 0, 0, 0, 1;
T_sqrt << 0.9366, -0.0162, 0.3500, -0.2863, 0.0151, 0.9999, 0.0058, 0.0058, -0.3501, -0.0002, 0.9367, 0.0554, 0, 0, 0, 1;
std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > T_cams;
T_cams.push_back(T_base * T_sqrt.inverse());
std::cout << T_cams[0] << std::endl;
// fit quadric
Eigen::Vector3d sample = cloud->points[sample_index].getVector3fMap().cast<double>();
Quadric quadric(T_cams, cloud, sample, true);
quadric.fitQuadric(nn_indices);
Eigen::MatrixXi cam_source = Eigen::MatrixXi::Zero(1, cloud->points.size());
quadric.findTaubinNormalAxis(nn_indices, cam_source);
quadric.print();
// fit hand
tree->radiusSearch(cloud->points[sample_index], hand_radius, nn_indices, nn_dists);
Eigen::VectorXi pts_cam_source = Eigen::VectorXi::Ones(1, cloud->size());
Eigen::Matrix3Xd nn_normals(3, nn_indices.size());
Eigen::VectorXi nn_cam_source(nn_indices.size());
Eigen::Matrix3Xd centered_neighborhood(3, nn_indices.size());
nn_normals.setZero();
for (int j = 0; j < nn_indices.size(); j++)
{
nn_cam_source(j) = pts_cam_source(nn_indices[j]);
centered_neighborhood.col(j) = cloud->points[nn_indices[j]].getVector3fMap().cast<double>() - sample;
}
double finger_width_ = 0.01;
double hand_outer_diameter_ = 0.09;
double hand_depth_ = 0.06;
ParallelHand finger_hand(finger_width_, hand_outer_diameter_, hand_depth_);
double hand_height_ = 0.02;
double init_bite_ = 0.01;
RotatingHand rotating_hand(T_cams[0].block(0, 3, 3, 1) - sample,
T_cams[0].block(0, 3, 3, 1) - sample, finger_hand, true, pts_cam_source(sample_index));
rotating_hand.transformPoints(centered_neighborhood, quadric.getNormal(), quadric.getCurvatureAxis(), nn_normals, nn_cam_source,
hand_height_);
std::vector<GraspHypothesis> h = rotating_hand.evaluateHand(init_bite_, sample, 1);
for (int i = 0; i < h.size(); i++)
{
std::cout << "-- orientation " << i << " --\n";
h[i].print();
}
// std::cout << "\n";
// rotating_hand.evaluateHand(init_bite_, sample, 1);
// rotating_hand.print();
return 0;
}
int main(int argc, char** argv)
{
double taubin_radius = 0.03; // radius of curvature-estimation neighborhood
std::string file = "/home/andreas/data/mlaffordance/round21l_reg.pcd";
PointCloud::Ptr cloud(new PointCloud);
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(file, *cloud) == -1) //* load the file
{
PCL_ERROR("Couldn't read input PCD file\n");
return (-1);
}
pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
std::vector<int> nn_outer_indices;
std::vector<float> nn_outer_dists;
pcl::KdTreeFLANN<pcl::PointXYZRGBA>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZRGBA>());
int sample_index = 0;
if (cloud->isOrganized())
{
organized_neighbor->setInputCloud(cloud);
organized_neighbor->radiusSearch(cloud->points[sample_index], taubin_radius, nn_outer_indices, nn_outer_dists);
}
else
{
tree->setInputCloud(cloud);
tree->radiusSearch(cloud->points[sample_index], taubin_radius, nn_outer_indices, nn_outer_dists);
}
Eigen::Matrix4d T_base, T_sqrt;
T_base << 0, 0.445417, 0.895323, 0.22, 1, 0, 0, -0.02, 0, 0.895323, -0.445417, 0.24, 0, 0, 0, 1;
T_sqrt << 0.9366, -0.0162, 0.3500, -0.2863, 0.0151, 0.9999, 0.0058, 0.0058, -0.3501, -0.0002, 0.9367, 0.0554, 0, 0, 0, 1;
std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > T_cams;
T_cams.push_back(T_base * T_sqrt.inverse());
Eigen::Vector3d sample = cloud->points[sample_index].getVector3fMap().cast<double>();
Quadric quadric(T_cams, cloud, sample, true);
quadric.fitQuadric(nn_outer_indices);
Eigen::MatrixXi cam_source = Eigen::MatrixXi::Zero(1, cloud->points.size());
quadric.findTaubinNormalAxis(nn_outer_indices, cam_source);
quadric.print();
std::set<Eigen::Vector3i, VectorComparator> s;
Eigen::Matrix3i M;
M << 1,1,1,2,3,4,1,1,1;
for (int i=0; i < M.rows(); i++)
s.insert(M.row(i));
std::cout << "M:" << std::endl;
std::cout << M << std::endl;
Eigen::Matrix<int, Eigen::Dynamic, 3> N(s.size(), 3);
int i = 0;
for (std::set<Eigen::Vector3i, VectorComparator>::iterator it=s.begin(); it!=s.end(); ++it)
{
N.row(i) = *it;
i++;
}
std::cout << "N:" << std::endl;
std::cout << N << std::endl;
return 0;
}
void cubic
(
double a3, double b2, double c1, double d0, char opt,
double& r1r, double& r1i, double& r2r, double& r2i, double& r3r, double& r3i
)
{
const double rad = 57.29577951308230;
const double onethird = 1.0/3.0;
const double small = 0.00000001;
double temp1, temp2, p, q, r, delta, e0, cosphi, sinphi, phi;
// ------------------------ implementation --------------------------
r1r = 0.0;
r1i = 0.0;
r2r = 0.0;
r2i = 0.0;
r3r = 0.0;
r3i = 0.0;
if (fabs(a3) > small)
{
// ------------- force coefficients into std form -------------------
p= b2/a3;
q= c1/a3;
r= d0/a3;
a3= onethird*( 3.0 *q - p*p );
b2= (1.0 /27.0 )*( 2.0 *p*p*p - 9.0 *p*q + 27.0 *r );
delta= (a3*a3*a3/27.0 ) + (b2*b2*0.25 );
// -------------------- use cardans formula ------------------------
if ( delta > small )
{
temp1= (-b2*0.5 )+sqrt(delta);
temp2= (-b2*0.5 )-sqrt(delta);
temp1= sgn(temp1)*pow( fabs(temp1),onethird );
temp2= sgn(temp2)*pow( fabs(temp2),onethird );
r1r= temp1 + temp2 - p*onethird;
if (opt=='I')
{
r2r= -0.5 *(temp1 + temp2) - p*onethird;
r2i= -0.5 *sqrt( 3.0 )*(temp1 - temp2);
r3r= -0.5 *(temp1 + temp2) - p*onethird;
r3i= -r2i;
}
else
{
r2r= 99999.9;
r3r= 99999.9;
}
}
else
{
// -------------------- evaluate zero point ------------------------
if ( fabs( delta ) < small )
{
r1r= -2.0*sgn(b2)*pow(fabs(b2*0.5),onethird) - p*onethird;
r2r= sgn(b2)*pow(fabs(b2*0.5),onethird) - p*onethird;
if (opt=='U')
r3r= 99999.9;
else
r3r= r2r;
}
else
{
// --------------- use trigonometric identities ----------------
e0 = 2.0 *sqrt(-a3*onethird);
cosphi = (-b2/(2.0 *sqrt(-a3*a3*a3/27.0 )) );
sinphi = sqrt( 1.0 -cosphi*cosphi );
phi = atan2( sinphi,cosphi );
if (phi < 0.0)
phi = phi + 2.0*Pi;
r1r= e0*cos( phi*onethird ) - p*onethird;
r2r= e0*cos( phi*onethird + 120.0 /rad ) - p*onethird;
r3r= e0*cos( phi*onethird + 240.0 /rad ) - p*onethird;
} // if fabs(delta)...
} // if delta > small
} // if fabs > small
else
{
quadric( b2, c1, d0, opt, r1r, r1i, r2r, r2i );
r3r = 99999.9;
r3i = 99999.9;
}
}
