int find_closest(t_data *data, const int get_normal)
{
data->closest[1] = -1;
if (get_normal == 1)
{
find_closest_sphere(data, data->viewray, &(data->t));
find_closest_plane(data, data->viewray, &(data->t));
find_closest_cone(data, data->viewray, &(data->t));
find_closest_cylinder(data, data->viewray, &(data->t));
if (data->closest[1] == -1)
return (0);
if (data->closest[0] == SPHERE)
return (sphere_normal(data));
else if (data->closest[0] == PLANE)
return (plane_normal(data));
else if (data->closest[0] == CYLINDER)
return (cylinder_normal(data));
else if (data->closest[0] == CONE)
return (cone_normal(data));
return (1);
}
return (hit_any_sphere(data, data->lightray, data->t) ||
hit_any_plane(data, data->lightray, data->t) ||
hit_any_cone(data, data->lightray, data->t) ||
hit_any_cylinder(data, data->lightray, data->t));
}
/*****************************************************************
double* excess_of_quad(int ni, int nj, double *vec1, double *vec2,
double *vec3, double *vec4 )
*******************************************************************/
double* excess_of_quad(int ni, int nj, double *vec1, double *vec2, double *vec3, double *vec4 )
{
int n;
double ang12, ang23, ang34, ang41;
double *excess, *plane1, *plane2, *plane3, *plane4;
double *angle12, *angle23, *angle34, *angle41;
excess = (double *)malloc(ni*nj*sizeof(double));
plane1=plane_normal(ni, nj, vec1, vec2);
plane2=plane_normal(ni, nj, vec2, vec3);
plane3=plane_normal(ni, nj, vec3, vec4);
plane4=plane_normal(ni, nj, vec4, vec1);
angle12=angle_between_vectors(ni, nj, plane2,plane1);
angle23=angle_between_vectors(ni, nj, plane3,plane2);
angle34=angle_between_vectors(ni, nj, plane4,plane3);
angle41=angle_between_vectors(ni, nj, plane1,plane4);
for(n=0; n<ni*nj; n++) {
ang12 = M_PI-angle12[n];
ang23 = M_PI-angle23[n];
ang34 = M_PI-angle34[n];
ang41 = M_PI-angle41[n];
excess[n] = ang12+ang23+ang34+ang41-2*M_PI;
}
free(plane1);
free(plane2);
free(plane3);
free(plane4);
free(angle12);
free(angle23);
free(angle34);
free(angle41);
return excess;
}; /* excess_of_quad */
bool ObjectFinder::find_toy_block(float surface_height, geometry_msgs::PoseStamped &object_pose) {
Eigen::Vector3f plane_normal;
double plane_dist;
//bool valid_plane;
Eigen::Vector3f major_axis;
Eigen::Vector3f centroid;
bool found_object = true; //should verify this
double block_height = 0.035; //this height is specific to the TOY_BLOCK model
//if insufficient points in plane, find_plane_fit returns "false"
//should do more sanity testing on found_object status
//hard-coded search bounds based on a block of width 0.035
found_object = pclUtils_.find_plane_fit(0.4, 1, -0.5, 0.5, surface_height + 0.025, surface_height + 0.045, 0.001,
plane_normal, plane_dist, major_axis, centroid);
//should have put a return value on find_plane_fit;
//
if (plane_normal(2) < 0) plane_normal(2) *= -1.0; //in world frame, normal must point UP
Eigen::Matrix3f R;
Eigen::Vector3f y_vec;
R.col(0) = major_axis;
R.col(2) = plane_normal;
R.col(1) = plane_normal.cross(major_axis);
Eigen::Quaternionf quat(R);
object_pose.header.frame_id = "base_link";
object_pose.pose.position.x = centroid(0);
object_pose.pose.position.y = centroid(1);
//the TOY_BLOCK model has its origin in the middle of the block, not the top surface
//so lower the block model origin by half the block height from upper surface
object_pose.pose.position.z = centroid(2)-0.5*block_height;
//create R from normal and major axis, then convert R to quaternion
object_pose.pose.orientation.x = quat.x();
object_pose.pose.orientation.y = quat.y();
object_pose.pose.orientation.z = quat.z();
object_pose.pose.orientation.w = quat.w();
return found_object;
}
static void
do_normal(GLfloat x1, GLfloat y1, GLfloat z1,
GLfloat x2, GLfloat y2, GLfloat z2,
GLfloat x3, GLfloat y3, GLfloat z3)
{
plane plane;
vector n;
vector_set(&plane.p1, x1, y1, z1);
vector_set(&plane.p2, x2, y2, z2);
vector_set(&plane.p3, x3, y3, z3);
plane_normal(plane, &n);
n.x = -n.x; n.y = -n.y; n.z = -n.z;
glNormal3f(n.x, n.y, n.z);
#ifdef DEBUG
/* Draw a line in the direction of this face's normal. */
{
GLfloat ax = n.x > 0 ? n.x : -n.x;
GLfloat ay = n.y > 0 ? n.y : -n.y;
GLfloat az = n.z > 0 ? n.z : -n.z;
GLfloat mx = (x1 + x2 + x3) / 3;
GLfloat my = (y1 + y2 + y3) / 3;
GLfloat mz = (z1 + z2 + z3) / 3;
GLfloat xx, yy, zz;
GLfloat max = ax > ay ? ax : ay;
if (az > max) max = az;
max *= 2;
xx = n.x / max;
yy = n.y / max;
zz = n.z / max;
glBegin(GL_LINE_LOOP);
glVertex3f(mx, my, mz);
glVertex3f(mx+xx, my+yy, mz+zz);
glEnd();
}
#endif /* DEBUG */
}
bool Picker3d::pick(const glm::ivec2 &input, const float z,
glm::vec3 &pickedPosition) const {
if (!mData) return false;
ci::CameraPersp& cam(mData->mCamera);
// generate a ray from the camera into our world
float u = static_cast<float>(input.x) / mData->mWindowSize.x;
float v = static_cast<float>(input.y) / mData->mWindowSize.y;
// because OpenGL and Cinder use a coordinate system
// where (0, 0) is in the LOWERleft corner, we have to flip the v-coordinate
ci::Ray ray = cam.generateRay(u , 1.0f - v, cam.getAspectRatio() );
glm::vec3 plane_origin(glm::vec3(0.0f, 0.0f, z)),
plane_normal(glm::vec3(0.0f, 0.0f, 1.0f));
float t;
if (ray.calcPlaneIntersection(plane_origin, plane_normal, &t)) {
pickedPosition = ray.calcPosition(t);
return true;
}
return false;
}
void OnePointRansac::planesCheck(
pcl::PointCloud<PointType>::Ptr& cloud,
pcl::PointCloud<NormalType>::Ptr& cloud_normals,
std::vector<int>& filtered_indices,
std::vector<int>& planes_indices,
float max_angle,
int map_min_inliers
){
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients model_coefficients;
inliers->indices.clear ();
model_coefficients.values.clear ();
inliers->header = model_coefficients.header = cloud->header;
model_coefficients.values.resize (4);
size_t best_score = 0;
const size_t no_of_pts = cloud->points.size();
const double no_of_pts_inverse = 1.0/(double)no_of_pts;
const double lognum = std::log(1-this->probability);
//Random
srand (time(NULL));
unsigned current_max_iterations = this->max_iterations;
unsigned it = 0;
for(; it < current_max_iterations; ++it)
{
const int idx = rand() % no_of_pts;
const Eigen::Vector3f& current_plane_normal = cloud_normals->points[idx].getNormalVector3fMap ();
float current_plane_offset = current_plane_normal.dot(cloud->points[idx].getVector3fMap ());
size_t current_score = 0;
for (size_t k = 0; k < no_of_pts; ++k)
{
const int kidx = k;
if (current_plane_normal.dot (cloud_normals->points[kidx].getNormalVector3fMap ()) > this->min_cosangle_th &&
(((current_plane_offset - current_plane_normal.dot (cloud->points[kidx].getVector3fMap ())) /
current_plane_normal.dot (cloud_normals->points[kidx].getNormalVector3fMap ())) *
cloud_normals->points[kidx].getNormalVector3fMap ()).squaredNorm () < this->distance_th)
++current_score;
}
// update
if(current_score > best_score)
{
best_score = current_score;
model_coefficients.values[0] = current_plane_normal.x ();
model_coefficients.values[1] = current_plane_normal.y ();
model_coefficients.values[2] = current_plane_normal.z ();
model_coefficients.values[3] = current_plane_offset;
if(best_score == no_of_pts)
current_max_iterations = 0;
else
current_max_iterations = std::min((double)this->max_iterations, std::ceil(lognum / std::log(1.0 - std::pow((double)best_score * no_of_pts_inverse, 3.0))));
}
}
Eigen::Map<Eigen::Vector3f> plane_normal (&(model_coefficients.values[0]));
for (size_t k = 0; k < no_of_pts; ++k)
{
const int kidx = k;
if (plane_normal.dot (cloud_normals->points[kidx].getNormalVector3fMap ()) > this->min_cosangle_th &&
(((model_coefficients.values[3] - plane_normal.dot (cloud->points[kidx].getVector3fMap ())) /
plane_normal.dot (cloud_normals->points[kidx].getNormalVector3fMap ())) *
cloud_normals->points[kidx].getNormalVector3fMap ()).squaredNorm () < this->distance_th)
inliers->indices.push_back (kidx);
}
pcl::ExtractIndices<PointType> extract;
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (planes_indices);
extract.setNegative (true);
extract.filter (filtered_indices);
}
void ClusterExtraction::processCloud(float plane_tolerance)
{
ros::Time stamp = ros::Time::now();
if(!pcl_data)
{
ROS_INFO("No xtion_camera data.");
sleep(1);
return;
}
//pcl::PointCloud<PoinT>::Ptr _cloud;// (new pcl::PointCloud<PoinT>);
sensor_msgs::PointCloud2ConstPtr _temp_cloud_ = pcl_data;
pcl::PointCloud<PoinT>::Ptr _cloud (new pcl::PointCloud<PoinT>);
pcl::fromROSMsg(*_temp_cloud_, *_cloud);
pcl::VoxelGrid<PoinT> vg_filter;
pcl::PointCloud<PoinT>::Ptr cloud_filtered (new pcl::PointCloud<PoinT>);
vg_filter.setInputCloud (_cloud);
vg_filter.setLeafSize (0.01f, 0.01f, 0.01f);
vg_filter.filter (*cloud_filtered);
_cloud = cloud_filtered;
/**********************************************
* NEW BULL
*********************************************/
//////////////////////////////////////////////////////////////////////
// Cluster Extraction
//////////////////////////////////////////////////////////////////////
// Findout the points that are more than 1.25 m away.
pcl::PointIndices::Ptr out_of_range_points (new pcl::PointIndices);
unsigned int i = 0;
BOOST_FOREACH(const PoinT& pt, _cloud->points)
{
if(sqrt( (pt.x*pt.x) + (pt.y*pt.y) + (pt.z*pt.z) ) > 1.5 || isnan (pt.x) || isnan (pt.y) || isnan (pt.z) ||
isinf (pt.x) || isinf (pt.y) || isinf (pt.z) )
out_of_range_points->indices.push_back(i);
i++;
}
pcl::ExtractIndices<PoinT> extract;
pcl::PointCloud<PoinT>::Ptr cloud (new pcl::PointCloud<PoinT>);
// Perform the extraction of these points (indices).
extract.setInputCloud(_cloud);
extract.setIndices(out_of_range_points);
extract.setNegative (true);
extract.filter (*cloud);
// Prepare plane segmentation.
pcl::SACSegmentation<PoinT> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<PoinT>::Ptr cloud_plane; // This door is opened elsewhere.
pcl::PointCloud<PoinT>::Ptr cloud_f (new pcl::PointCloud<PoinT> ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.02);
// Remove the planes.
i = 0;
int nr_points = (int) cloud->points.size();
tf::StampedTransform base_link_to_openni;
try
{
tf_listener->waitForTransform("base_link", cloud->header.frame_id, ros::Time(0), ros::Duration(1));
//tf_listener->transformPoint("base_link", plane_normal, _plane_normal);
tf_listener->lookupTransform("base_link", cloud->header.frame_id, ros::Time(0), base_link_to_openni);
}
catch(tf::TransformException& ex)
{
ROS_INFO("COCKED UP POINT INFO! Why: %s", ex.what());
}
// We do this here so that we can put in an empty cluster, if we see no table in the first place.
doro_msgs::ClusterArray __clusters;
while (cloud->points.size () > 0.5 * nr_points)
{
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
//std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_f);
// Is this a parallel to ground plane? If yes, save it.
tf::Vector3 plane_normal (coefficients->values[0], coefficients->values[1], coefficients->values[2]);
tf::Vector3 _plane_normal = base_link_to_openni*plane_normal;
// What's the angle between this vector and the actual z axis? cos_inverse ( j component )...
tf::Vector3 normal (_plane_normal.x(), _plane_normal.y(), _plane_normal.z());
normal = normal.normalized();
//std::cout<<"x: "<<normal.x()<<"\t";
//std::cout<<"y: "<<normal.y()<<"\t";
//std::cout<<"z: "<<normal.z()<<"\t";
if(acos (normal.z()) < plane_tolerance)
{
cloud_plane = pcl::PointCloud<PoinT>::Ptr(new pcl::PointCloud<PoinT>);
*cloud_plane = *cloud_f;
}
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud = *cloud_f;
}
if(!cloud_plane)
{
ROS_INFO("No table or table-like object could be seen. Can't extract...");
//clusters_pub.publish(__clusters);
//sleep(1);
return;
}
//ROS_INFO("Table seen.");
//table_cloud_pub.publish(cloud_plane);
//////////////////////////////////////////////////////////////////////
/**
* COMPUTE THE CENTROID OF THE PLANE AND PUBLISH IT.
*/
//////////////////////////////////////////////////////////////////////
Eigen::Vector4f plane_cen;
// REMOVE COMMENTS WITH REAL ROBOT!!!
pcl::compute3DCentroid(*cloud_plane, plane_cen);
//std::cout<< plane_cen;
tf::Vector3 plane_centroid (plane_cen[0], plane_cen[1], plane_cen[2]);
tf::Vector3 _plane_centroid = base_link_to_openni*plane_centroid;
geometry_msgs::PointStamped _plane_centroid_ROSMsg;
_plane_centroid_ROSMsg.header.frame_id = "base_link";
_plane_centroid_ROSMsg.header.stamp = stamp;
_plane_centroid_ROSMsg.point.x = _plane_centroid.x();
_plane_centroid_ROSMsg.point.y = _plane_centroid.y();
_plane_centroid_ROSMsg.point.z = _plane_centroid.z();
// Publish the centroid.
table_position_pub.publish(_plane_centroid_ROSMsg);
pcl::search::KdTree<PoinT>::Ptr tree (new pcl::search::KdTree<PoinT>);
if(cloud->points.size() == 0)
{
clusters_pub.publish(__clusters);
return;
}
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PoinT> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (20);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
//ROS_INFO("WIDTH: %d, HEIGHT: %d\n", _cloud->width, _cloud->height);
//ROS_INFO("GOOD TILL HERE!");
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<PoinT>::Ptr cloud_cluster (new pcl::PointCloud<PoinT>);
cloud_cluster->header.frame_id = cloud->header.frame_id;
long int color_r, color_g, color_b;
uint8_t mean_r, mean_g, mean_b;
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
{
cloud_cluster->points.push_back (cloud->points[*pit]);
/* ***************** */
/* COLOR COMPUTATION */
/* ***************** */
color_r += cloud->points[*pit].r;
color_g += cloud->points[*pit].g;
color_b += cloud->points[*pit].b;
}
geometry_msgs::Point a, b;
std::vector <double> cluster_dims = getClusterDimensions(cloud_cluster, a, b);
mean_r = (uint8_t) (color_r / cloud_cluster->points.size ());
mean_g = (uint8_t) (color_g / cloud_cluster->points.size ());
mean_b = (uint8_t) (color_b / cloud_cluster->points.size ());
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
cloud_cluster->header.frame_id = cloud->header.frame_id;
Eigen::Vector4f cluster_cen;
pcl::compute3DCentroid(*cloud_cluster, cluster_cen);
tf::Vector3 cluster_centroid (cluster_cen[0], cluster_cen[1], cluster_cen[2]);
tf::Vector3 _cluster_centroid = base_link_to_openni*cluster_centroid;
geometry_msgs::PointStamped _cluster_centroid_ROSMsg;
_cluster_centroid_ROSMsg.header.frame_id = "base_link";
_cluster_centroid_ROSMsg.header.stamp = stamp;
_cluster_centroid_ROSMsg.point.x = _cluster_centroid.x();
_cluster_centroid_ROSMsg.point.y = _cluster_centroid.y();
_cluster_centroid_ROSMsg.point.z = _cluster_centroid.z();
if(DIST(cluster_centroid,plane_centroid) < 0.6 && _cluster_centroid.z() > _plane_centroid.z())
{
doro_msgs::Cluster __cluster;
__cluster.centroid = _cluster_centroid_ROSMsg;
// Push cluster dimentions. Viewed width, breadth and height
__cluster.cluster_size = cluster_dims;
__cluster.a = a;
__cluster.b = b;
// Push colors
__cluster.color.push_back(mean_r);
__cluster.color.push_back(mean_g);
__cluster.color.push_back(mean_b);
__clusters.clusters.push_back (__cluster);
j++;
}
}
clusters_pub.publish(__clusters);
/////////////// RUBBISH ENDS ////////////////
//if(pcl_data)
// pcl_data.reset();
}
/* Accepts a range of inverted tris. Refacets affected surface so that no tris
are inverted. */
MBErrorCode remove_inverted_tris(MBTag normal_tag, MBRange tris, const bool debug ) {
MBErrorCode result;
bool failures_occur = false;
while(!tris.empty()) {
/* Get a group of triangles to re-facet. They must be adjacent to each other
and in the same surface. */
MBRange tris_to_refacet;
tris_to_refacet.insert( tris.front() );
MBRange surf_set;
result = MBI()->get_adjacencies( tris_to_refacet, 4, false, surf_set );
assert(MB_SUCCESS == result);
if(1 != surf_set.size()) {
std::cout << "remove_inverted_tris: tri is in " << surf_set.size()
<< " surfaces" << std::endl;
return MB_FAILURE;
}
// get all tris in the surface
MBRange surf_tris;
result = MBI()->get_entities_by_type( surf_set.front(), MBTRI, surf_tris );
assert(MB_SUCCESS == result);
/* Find all of the adjacent inverted triangles of the same surface. Keep
searching until a search returns no new triangles. */
bool search_again = true;
while(search_again) {
// Here edges are being created. Remember to delete them. Outside of this
// function. Skinning gets bogged down if unused MBEdges (from other
// surfaces) accumulate.
MBRange tri_edges;
result = MBI()->get_adjacencies( tris_to_refacet, 1, true, tri_edges,
MBInterface::UNION );
assert(MB_SUCCESS == result);
MBRange connected_tris;
result = MBI()->get_adjacencies( tri_edges, 2, false, connected_tris,
MBInterface::UNION );
assert(MB_SUCCESS == result);
result = MBI()->delete_entities( tri_edges );
assert(MB_SUCCESS == result);
MBRange tris_to_refacet2 = intersect( tris_to_refacet, connected_tris );
tris_to_refacet2 = intersect( tris_to_refacet, surf_tris );
if(tris_to_refacet.size() == tris_to_refacet2.size()) search_again = false;
tris_to_refacet.swap( tris_to_refacet2 );
}
// Remove the inverted tris that will be refaceted.
tris = subtract( tris, tris_to_refacet );
// do edges already exist?
MBRange temp;
result = MBI()->get_entities_by_type(0, MBEDGE, temp );
assert(MB_SUCCESS == result);
if(!temp.empty()) MBI()->list_entities( temp );
assert(temp.empty());
// keep enlarging patch until we have tried to refacet the entire surface
int counter=0;
while(true) {
// do edges already exist?
temp.clear();
result = MBI()->get_entities_by_type(0, MBEDGE, temp );
assert(MB_SUCCESS == result);
if(!temp.empty()) MBI()->list_entities( temp );
assert(temp.empty());
counter++;
// Only try enlarging each patch a few times
if(48 == counter) {
failures_occur = true;
if(debug) std::cout << "remove_inverted_tris: ear clipping failed, counter="
<< counter << std::endl;
break;
}
// THIS PROVIDES A BAD EXIT. MUST FIX
// get the edges of the patch of inverted tris
MBRange tri_edges;
result = MBI()->get_adjacencies( tris_to_refacet, 1, true, tri_edges,
MBInterface::UNION );
assert(MB_SUCCESS == result);
// get all adjacent tris to the patch of inverted tris in the surface
MBRange adj_tris;
result = MBI()->get_adjacencies( tri_edges, 2, false, adj_tris,
MBInterface::UNION );
assert(MB_SUCCESS == result);
result = MBI()->delete_entities( tri_edges );
assert(MB_SUCCESS == result);
tris_to_refacet = intersect( surf_tris, adj_tris );
if(tris_to_refacet.empty()) continue;
//gen::print_triangles( tris_to_refacet );
// get an area-weighted normal of the adj_tris
MBCartVect plane_normal(0,0,0);
//for(unsigned int i=0; i<tris_to_refacet.size(); i++) {
for(MBRange::iterator i=tris_to_refacet.begin(); i!=tris_to_refacet.end(); i++) {
MBCartVect norm;
result = MBI()->tag_get_data( normal_tag, &(*i), 1, &norm);
assert(MB_SUCCESS == result);
double area;
result = gen::triangle_area( *i, area );
assert(MB_SUCCESS == result);
if(debug) std::cout << "norm=" << norm << " area=" << area << std::endl;
//plane_normal += norm*area;
plane_normal += norm;
}
plane_normal.normalize();
// do edges already exist?
temp.clear();
result = MBI()->get_entities_by_type(0, MBEDGE, temp );
assert(MB_SUCCESS == result);
if(!temp.empty()) MBI()->list_entities( temp );
assert(temp.empty());
// skin the tris
MBRange unordered_edges;
//MBSkinner tool(MBI());
//result = tool.find_skin( tris_to_refacet, 1, unordered_edges, false );
result = gen::find_skin( tris_to_refacet, 1, unordered_edges, false );
assert(MB_SUCCESS == result);
if(unordered_edges.empty()) {
// do edges already exist?
MBRange temp;
result = MBI()->get_entities_by_type(0, MBEDGE, temp );
assert(MB_SUCCESS == result);
if(!temp.empty()) MBI()->list_entities( temp );
assert(temp.empty());
continue;
}
//std::cout << "remove_inverted_tris: surf_id="
// << gen::geom_id_by_handle(surf_set.front()) << std::endl;
//result = MBI()->list_entities( tris_to_refacet );
//assert(MB_SUCCESS == result);
// assemble into a polygon
std::vector<MBEntityHandle> polygon_of_verts;
result = arc::order_verts_by_edge( unordered_edges, polygon_of_verts );
if(debug) gen::print_loop( polygon_of_verts );
//assert(MB_SUCCESS == result);
if(MB_SUCCESS != result) {
if(debug) std::cout << "remove_inverted_tris: couldn't order polygon by edge" << std::endl;
return MB_FAILURE;
}
// remember to remove edges
result = MBI()->delete_entities( unordered_edges );
assert(MB_SUCCESS == result);
// remove the duplicate endpt
polygon_of_verts.pop_back();
// the polygon should have at least 3 verts
if(3 > polygon_of_verts.size()) {
if(debug) std::cout << "remove_inverted_tris: polygon has too few points" << std::endl;
return MB_FAILURE;
}
// orient the polygon with the triangles (could be backwards)
// get the first adjacent tri
MBEntityHandle edge[2] = { polygon_of_verts[0], polygon_of_verts[1] };
MBRange one_tri;
result = MBI()->get_adjacencies( edge, 2, 2, false, one_tri );
assert(MB_SUCCESS == result);
one_tri = intersect( tris_to_refacet, one_tri );
assert(1 == one_tri.size());
const MBEntityHandle *conn;
int n_conn;
result = MBI()->get_connectivity( one_tri.front(), conn, n_conn );
assert(MB_SUCCESS == result);
assert(3 == n_conn);
if( (edge[0]==conn[1] && edge[1]==conn[0]) ||
(edge[0]==conn[2] && edge[1]==conn[1]) ||
(edge[0]==conn[0] && edge[1]==conn[2]) ) {
reverse( polygon_of_verts.begin(), polygon_of_verts.end() );
if(debug) std::cout << "remove_inverted_tris: polygon reversed" << std::endl;
}
/* facet the polygon. Returns MB_FAILURE if it fails to facet the polygon. */
MBRange new_tris;
result = gen::ear_clip_polygon( polygon_of_verts, plane_normal, new_tris );
// break if the refaceting is successful
if(MB_SUCCESS == result) {
// summarize tri area
for(MBRange::iterator i=new_tris.begin(); i!=new_tris.end(); i++) {
double area;
result = gen::triangle_area( *i, area );
assert(MB_SUCCESS == result);
if(debug) std::cout << " new tri area=" << area << std::endl;
}
// check the new normals
std::vector<MBCartVect> new_normals;
result = gen::triangle_normals( new_tris, new_normals );
if(MB_SUCCESS != result) return result;
// test the new triangles
std::vector<int> inverted_tri_indices;
std::vector<MBCartVect> normals ( new_normals.size(), plane_normal );
result = zip::test_normals( normals, new_normals, inverted_tri_indices );
assert(MB_SUCCESS == result);
if(inverted_tri_indices.empty()) {
// remove the tris that were re-faceted
tris = subtract( tris, tris_to_refacet );
result = MBI()->remove_entities( surf_set.front(), tris_to_refacet );
assert(MB_SUCCESS == result);
result = MBI()->delete_entities( tris_to_refacet );
assert(MB_SUCCESS == result);
// add the new tris to the surf set
result = MBI()->add_entities( surf_set.front(), new_tris );
assert(MB_SUCCESS == result);
// put the new normals on the new tris
result = gen::save_normals( new_tris, normal_tag );
assert(MB_SUCCESS == result);
if(debug) std::cout << "remove_inverted_tris: success fixing a patch" << std::endl;
break;
}
}
// remember to delete the tris that were created from the failed ear clipping
else {
result = MBI()->delete_entities( new_tris );
assert(MB_SUCCESS == result);
}
// If the entire surface could not be ear clipped, give up
if (tris_to_refacet.size() == surf_tris.size()) {
if(debug) std::cout << "remove_inverted_tris: ear clipping entire surface failed"
<< std::endl;
return MB_FAILURE;
}
} // loop until the entire surface has attempted to be refaceted
} // loop over each patch of inverted tris
if(failures_occur) {
if(debug) std::cout << "remove_inverted_facets: at least one failure occured" << std::endl;
return MB_FAILURE;
} else {
return MB_SUCCESS;
}
}
void callback_planes(const vision_msgs::PlanesConstPtr& planes)
{
if (!_correct_lateral) {
_avg_plane_dist = 0;
_accumulated_plane_dists = 0;
return;
}
//find wall that is perpendicular to the robots direction
double max_dot = 0.0;
int ortho_plane = 0;
for(int i = 0; i < planes->planes.size(); ++i)
{
const vision_msgs::Plane& plane = planes->planes[i];
if (plane.is_ground_plane) continue;
Eigen::Vector3f plane_normal(plane.plane_coefficients[0],
plane.plane_coefficients[1],
plane.plane_coefficients[2]);
Eigen::Vector3f forward(1,0,0);
double dotprod = std::abs(forward.dot(plane_normal));
if (dotprod > max_dot) {
max_dot = dotprod;
ortho_plane = i;
}
}
if (max_dot > 0.9) {
_front_plane = planes->planes[ortho_plane];
_see_front_plane = true;
_iteration_lateral++;
double x_diff = get_x_diff();
ROS_ERROR("x diff = %.3lf",x_diff);
if (!std::isnan(x_diff)) {
_avg_plane_dist += x_diff;
_accumulated_plane_dists++;
}
if (_iteration_lateral >= _max_iterations()) {
if (_accumulated_plane_dists > 0) {
double avg_diff = _avg_plane_dist / (double)_accumulated_plane_dists;
ROS_ERROR("Attempt to correct position based on wall");
if (std::abs(avg_diff) < 0.1) {
double dx = cos(_theta);
double dy = sin(_theta);
double new_x = _x + avg_diff*dx;
double new_y = _y + avg_diff*dy;
ROS_ERROR("corrected position (%.3lf,%.3lf) -> (%.3lf,%.3lf)", _x, _y, new_x, new_y);
_x = new_x;
_y = new_y;
}
}
_iteration_lateral = 0;
_avg_plane_dist = 0;
_accumulated_plane_dists = 0;
_correct_lateral = false;
}
}
else {
_see_front_plane = false;
if (_correct_lateral) _correct_lateral = false;
}
}
