template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}
distances.resize (indices_->size ());
Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float opening_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = apex + k * axis_dir;
Eigen::Vector4f dir = pt - pt_proj;
dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pt_proj;
float actual_cone_radius = tanf (opening_angle) * height.norm ();
height.normalize ();
// Calculate the cones perfect normals
Eigen::Vector4f cone_normal = sinf (opening_angle) * height + cosf (opening_angle) * dir;
// Aproximate the distance from the point to the cone as the difference between
// dist(point,cone_axis) and actual cone radius
double d_euclid = fabs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, cone_normal));
d_normal = (std::min) (d_normal, M_PI - d_normal);
distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
}
}
template <typename PointT> bool
pcl::SampleConsensusModelParallelPlane<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
// Needs a valid model coefficients
if (model_coefficients.size () != 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelParallelPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
return (false);
}
// Check against template, if given
if (eps_angle_ > 0.0)
{
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
coeff[3] = 0;
coeff.normalize ();
Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
if (fabs (axis.dot (coeff)) < cos_angle_)
return (false);
}
return (true);
}
void Plane::transform(Eigen::Matrix4f t){
Eigen::Vector4f n = t*Eigen::Vector4f(normal_x,normal_y,normal_z,0);//Only use rotational component
n.normalize();
normal_x = n(0);
normal_y = n(1);
normal_z = n(2);
Eigen::Vector4f p = t*Eigen::Vector4f(point_x,point_y,point_z,1);
point_x = p(0);
point_y = p(1);
point_z = p(2);
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
int nr_p = 0;
inliers.resize (indices_->size ());
error_sqr_dists_.resize (indices_->size ());
Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float ptdotdir = line_pt.dot (line_dir);
float dirdotdir = 1.0f / line_dir.dot (line_dir);
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < indices_->size (); ++i)
{
// Approximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = line_pt + k * line_dir;
Eigen::Vector4f dir = pt - pt_proj;
dir.normalize ();
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
error_sqr_dists_[nr_p] = distance;
++nr_p;
}
}
inliers.resize (nr_p);
error_sqr_dists_.resize (nr_p);
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPointToCylinder (
const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj)
{
Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) * line_dir.dot (line_dir);
pt_proj = line_pt + k * line_dir;
Eigen::Vector4f dir = pt - pt_proj;
dir.normalize ();
// Calculate the projection of the point onto the cylinder
pt_proj += dir * model_coefficients[6];
}
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::doSamplesVerifyModel (
const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
{
// Needs a valid model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
return (false);
}
Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float openning_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
{
Eigen::Vector4f pt (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z, 0);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = apex + k * axis_dir;
Eigen::Vector4f dir = pt - pt_proj;
dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pt_proj;
double actual_cone_radius = tan (openning_angle) * height.norm ();
// Aproximate the distance from the point to the cone as the difference between
// dist(point,cone_axis) and actual cone radius
if (fabs (static_cast<double>(pointToAxisDistance (pt, model_coefficients) - actual_cone_radius)) > threshold)
return (false);
}
return (true);
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}
distances.resize (indices_->size ());
Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float ptdotdir = line_pt.dot (line_dir);
float dirdotdir = 1.0f / line_dir.dot (line_dir);
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < indices_->size (); ++i)
{
// Aproximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
// @note need to revise this.
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = line_pt + k * line_dir;
Eigen::Vector4f dir = pt - pt_proj;
dir.normalize ();
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
}
}
template <typename PointInT> double
pcl::tracking::NormalCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target)
{
Eigen::Vector4f n = source.getNormalVector4fMap ();
Eigen::Vector4f n_dash = target.getNormalVector4fMap ();
if ( n.norm () <= 1e-5 || n_dash.norm () <= 1e-5 )
{
PCL_ERROR("norm might be ZERO!\n");
std::cout << "source: " << source << std::endl;
std::cout << "target: " << target << std::endl;
exit (1);
return 0.0;
}
else
{
n.normalize ();
n_dash.normalize ();
double theta = pcl::getAngle3D (n, n_dash);
if (!pcl_isnan (theta))
return 1.0 / (1.0 + weight_ * theta * theta);
else
return 0.0;
}
}
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
// normalize the vector perpendicular to the plane...
mc.normalize ();
// ... and store the resulting normal as a local copy of the model coefficients
Eigen::Vector4f tmp_mc = model_coefficients;
tmp_mc[0] = mc[0];
tmp_mc[1] = mc[1];
tmp_mc[2] = mc[2];
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < input_->points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
// Calculate the distance from the point to the plane
Eigen::Vector4f p (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
1);
// use normalized coefficients to calculate the scalar projection
float distance_to_plane = tmp_mc.dot (p);
pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
pp.matrix () = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = static_cast<uint32_t> (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
// Calculate the distance from the point to the plane
Eigen::Vector4f p (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
1);
// use normalized coefficients to calculate the scalar projection
float distance_to_plane = tmp_mc.dot (p);
pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
pp.matrix () = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe
}
}
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float opening_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < projected_points.points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the cone
for (size_t i = 0; i < inliers.size (); ++i)
{
Eigen::Vector4f pt (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
1);
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
pp.matrix () = apex + k * axis_dir;
Eigen::Vector4f dir = pt - pp;
dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pp;
float actual_cone_radius = tanf (opening_angle) * height.norm ();
// Calculate the projection of the point onto the cone
pp += dir * actual_cone_radius;
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = static_cast<uint32_t> (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the cone
for (size_t i = 0; i < inliers.size (); ++i)
{
pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
pcl::Vector4fMapConst pt = input_->points[inliers[i]].getVector4fMap ();
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
// Calculate the projection of the point on the line
pp.matrix () = apex + k * axis_dir;
Eigen::Vector4f dir = pt - pp;
dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pp;
float actual_cone_radius = tanf (opening_angle) * height.norm ();
// Calculate the projection of the point onto the cone
pp += dir * actual_cone_radius;
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "occlusion_culling_test");
ros::NodeHandle n;
ros::Publisher pub1 = n.advertise<sensor_msgs::PointCloud2>("original_point_cloud", 100);
ros::Publisher pub2 = n.advertise<sensor_msgs::PointCloud2>("occlusion_free_cloud", 100);
ros::Publisher pub3 = n.advertise<sensor_msgs::PointCloud2>("ray_points", 100);
ros::Publisher pub4 = n.advertise<visualization_msgs::Marker>("box_line_intersection", 10);
ros::Publisher pub5 = n.advertise<visualization_msgs::Marker>("sensor_origin", 1);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr occlusionFreeCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rayCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::string path = ros::package::getPath("component_test");
pcl::io::loadPCDFile<pcl::PointXYZ> (path+"/src/pcd/sphere_densed.pcd", *cloud);
Eigen::Vector3d a(-3,0,0);
Eigen::Affine3d pose;
pose.translation() = a;
geometry_msgs::Pose output_vector;
tf::poseEigenToMsg(pose, output_vector);
tf::Quaternion orientation = tf::createQuaternionFromRPY(0,0,0);;
Eigen::Quaterniond q;
geometry_msgs::Quaternion quet;
tf::quaternionTFToEigen(orientation, q);
tf::quaternionTFToMsg(orientation,quet);
pose.translation() = a;
visualization_msgs::Marker marker;
//*****************voxel grid occlusion estimation *****************
Eigen::Quaternionf quat(q.w(),q.x(),q.y(),q.z());
cloud->sensor_origin_ = Eigen::Vector4f(a[0],a[1],a[2],0);
cloud->sensor_orientation_= quat;
pcl::VoxelGridOcclusionEstimationT voxelFilter;
voxelFilter.setInputCloud (cloud);
//voxelFilter.setLeafSize (0.03279f, 0.03279f, 0.03279f);
voxelFilter.setLeafSize (0.04f, 0.04f, 0.04f);
//voxelFilter.filter(*cloud); // This filter doesn't really work, it introduces points inside the sphere !
voxelFilter.initializeVoxelGrid();
int state,ret;
Eigen::Vector3i t;
pcl::PointXYZ pt,p1,p2;
pcl::PointXYZRGB point;
std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > out_ray;
std::vector<geometry_msgs::Point> lineSegments;
geometry_msgs::Point linePoint;
Eigen::Vector3i min_b = voxelFilter.getMinBoxCoordinates ();
Eigen::Vector3i max_b = voxelFilter.getMaxBoxCoordinates ();
int count = 0;
bool foundBug = false;
// iterate over the entire voxel grid
for (int kk = min_b.z (); kk <= max_b.z () && !foundBug; ++kk)
{
for (int jj = min_b.y (); jj <= max_b.y () && !foundBug; ++jj)
{
for (int ii = min_b.x (); ii <= max_b.x () && !foundBug; ++ii)
{
Eigen::Vector3i ijk (ii, jj, kk);
// process all free voxels
int index = voxelFilter.getCentroidIndexAt (ijk);
Eigen::Vector4f centroid = voxelFilter.getCentroidCoordinate (ijk);
point = pcl::PointXYZRGB(0,244,0);
point.x = centroid[0];
point.y = centroid[1];
point.z = centroid[2];
//if(index!=-1 && point.x>1.2 && point.y<0.2 && point.y>-0.2)
if(index!=-1 )//&& point.x<-1.2 && point.y<0.2 && point.y>-0.2)
{
out_ray.clear();
ret = voxelFilter.occlusionEstimation( state,out_ray, ijk);
std::cout<<"State is:"<<state<<"\n";
/*
if(state == 1)
{
if(count++>=10)
{
foundBug = true;
break;
}
cout<<"Number of voxels in ray:"<<out_ray.size()<<"\n";
for(uint j=0; j< out_ray.size(); j++)
{
Eigen::Vector4f centroid = voxelFilter.getCentroidCoordinate (out_ray[j]);
pcl::PointXYZRGB p = pcl::PointXYZRGB(255,255,0);
p.x = centroid[0];
p.y = centroid[1];
p.z = centroid[2];
rayCloud->points.push_back(p);
std::cout<<"Ray X:"<<p.x<<" y:"<< p.y<<" z:"<< p.z<<"\n";
}
}
*/
if(state != 1)
{
// estimate direction to target voxel
Eigen::Vector4f direction = centroid - cloud->sensor_origin_;
direction.normalize ();
// estimate entry point into the voxel grid
float tmin = voxelFilter.rayBoxIntersection (cloud->sensor_origin_, direction,p1,p2);
if(tmin!=-1)
{
// coordinate of the boundary of the voxel grid
Eigen::Vector4f start = cloud->sensor_origin_ + tmin * direction;
linePoint.x = cloud->sensor_origin_[0]; linePoint.y = cloud->sensor_origin_[1]; linePoint.z = cloud->sensor_origin_[2];
//std::cout<<"Box Min X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
lineSegments.push_back(linePoint);
linePoint.x = start[0]; linePoint.y = start[1]; linePoint.z = start[2];
//std::cout<<"Box Max X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
lineSegments.push_back(linePoint);
linePoint.x = start[0]; linePoint.y = start[1]; linePoint.z = start[2];
//std::cout<<"Box Max X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
lineSegments.push_back(linePoint);
linePoint.x = centroid[0]; linePoint.y = centroid[1]; linePoint.z = centroid[2];
//std::cout<<"Box Max X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
lineSegments.push_back(linePoint);
rayCloud->points.push_back(point);
pt.x = centroid[0];
pt.y = centroid[1];
pt.z = centroid[2];
occlusionFreeCloud->points.push_back(pt);
}
}
}
}
}
}
/*
for ( int i = 0; i < (int)cloud->points.size(); i ++ )
{
pt = cloud->points[i];
t = voxelFilter.getGridCoordinates( pt.x, pt.y, pt.z);
// check if voxel is occupied, if empty then ignore
int index = voxelFilter.getCentroidIndexAt (t);
if (index = -1)
continue;
ret = voxelFilter.occlusionEstimation( state,out_ray, t);
if ( state == -1 )
{
std::cout<<"I am -1 negative !\n";
}
// estimate direction to target voxel
Eigen::Vector4f p = voxelFilter.getCentroidCoordinate (t);
Eigen::Vector4f direction = p - cloud->sensor_origin_;
direction.normalize ();
// estimate entry point into the voxel grid
float tmin = voxelFilter.rayBoxIntersection (cloud->sensor_origin_, direction,p1,p2);
if(tmin!=-1 && state != 1)
{
// coordinate of the boundary of the voxel grid
Eigen::Vector4f start = cloud->sensor_origin_ + tmin * direction;
linePoint.x = cloud->sensor_origin_[0]; linePoint.y = cloud->sensor_origin_[1]; linePoint.z = cloud->sensor_origin_[2];
// std::cout<<"Box Min X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
lineSegments.push_back(linePoint);
linePoint.x = start[0]; linePoint.y = start[1]; linePoint.z = start[2];
// std::cout<<"Box Max X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
lineSegments.push_back(linePoint);
// i,j,k coordinate of the voxel were the ray enters the voxel grid
Eigen::Vector3i ijk = voxelFilter.getGridCoordinates(start[0], start[1], start[2]);
// centroid coordinate of the entry voxel
Eigen::Vector4f voxel_max = voxelFilter.getCentroidCoordinate (ijk);
Eigen::Vector3f leaf_size_= voxelFilter.getLeafSize();
// std::cout<<"voxel_max X:"<<voxel_max[0]<<" y:"<< voxel_max[1]<<" z:"<< voxel_max[2]<<"\n";
if (direction[0] >= 0)
{
voxel_max[0] += leaf_size_[0] * 0.5f;
}
else
{
voxel_max[0] -= leaf_size_[0] * 0.5f;
}
if (direction[1] >= 0)
{
voxel_max[1] += leaf_size_[1] * 0.5f;
}
else
{
voxel_max[1] -= leaf_size_[1] * 0.5f;
}
if (direction[2] >= 0)
{
voxel_max[2] += leaf_size_[2] * 0.5f;
}
else
{
voxel_max[2] -= leaf_size_[2] * 0.5f;
}
// std::cout<<"voxel_max X:"<<voxel_max[0]<<" y:"<< voxel_max[1]<<" z:"<< voxel_max[2]<<"\n";
float t_max_x = tmin + (voxel_max[0] - start[0]) / direction[0];
float t_max_y = tmin + (voxel_max[1] - start[1]) / direction[1];
float t_max_z = tmin + (voxel_max[2] - start[2]) / direction[2];
// std::cout<<"t_max_x X:"<<t_max_x<<" y:"<< t_max_y<<" z:"<< t_max_z<<"\n";
float t_delta_x = leaf_size_[0] / static_cast<float> (fabs (direction[0]));
float t_delta_y = leaf_size_[1] / static_cast<float> (fabs (direction[1]));
float t_delta_z = leaf_size_[2] / static_cast<float> (fabs (direction[2]));
// std::cout<<"Direction X:"<<direction[0]<<" y:"<< direction[1]<<" z:"<< direction[2]<<"\n";
// std::cout<<"LeafSize X:"<<leaf_size_[0]<<" y:"<< leaf_size_[1]<<" z:"<< leaf_size_[2]<<"\n";
// std::cout<<"Delta X:"<<t_delta_x<<" y:"<< t_delta_y<<" z:"<< t_delta_z<<"\n";
linePoint.x = start[0]; linePoint.y = start[1]; linePoint.z = start[2];
// std::cout<<"Box Max X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
lineSegments.push_back(linePoint);
linePoint.x = pt.x; linePoint.y = pt.y; linePoint.z = pt.z;
// std::cout<<"Box Max X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
lineSegments.push_back(linePoint);
// linePoint.x = p1.x; linePoint.y = p1.y; linePoint.z = p1.z;
// std::cout<<"Box Min X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
// lineSegments.push_back(linePoint);
// linePoint.x = p2.x; linePoint.y = p2.y; linePoint.z = p2.z;
// std::cout<<"Box Max X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
// lineSegments.push_back(linePoint);
occlusionFreeCloud->points.push_back(pt);
}
if(count++<100 && pt.x>=-1.8 && pt.x<-1.1 && pt.y<0.4 && pt.y>-0.4)
{
for(uint j=0; j< out_ray.size(); j++)
{
point = pcl::PointXYZRGB(255,0,0);
Eigen::Vector4f centroid = voxelFilter.getCentroidCoordinate (out_ray[j]);
point.x = centroid[0];
point.y = centroid[1];
point.z = centroid[2];
rayCloud->points.push_back(point);
}
}
}
*/
visualization_msgs::Marker linesList = drawLines(lineSegments);
//*****************Rviz Visualization ************
ros::Rate loop_rate(10);
while (ros::ok())
{
//***marker publishing***
uint32_t shape = visualization_msgs::Marker::ARROW;
marker.type = shape;
marker.action = visualization_msgs::Marker::ADD;
//visulaization using the markers
marker.scale.x = 0.5;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
marker.ns = "basic_shapes";
marker.id = 2;
ROS_INFO("Publishing Marker");
// Set the frame ID and timestamp. See the TF tutorials for information on these.
marker.pose = output_vector;
marker.pose.orientation = quet;//output_vector.orientation;
marker.header.frame_id = "base_point_cloud";
marker.header.stamp = ros::Time::now();
marker.lifetime = ros::Duration(10);
// Publish the marker
pub5.publish(marker);
//***frustum cull and occlusion cull publish***
sensor_msgs::PointCloud2 cloud1;
sensor_msgs::PointCloud2 cloud2;
sensor_msgs::PointCloud2 cloud3;
pcl::toROSMsg(*cloud, cloud1);
pcl::toROSMsg(*occlusionFreeCloud, cloud2);
pcl::toROSMsg(*rayCloud, cloud3);
cloud1.header.frame_id = "base_point_cloud";
cloud2.header.frame_id = "base_point_cloud";
cloud3.header.frame_id = "base_point_cloud";
cloud1.header.stamp = ros::Time::now();
cloud2.header.stamp = ros::Time::now();
cloud3.header.stamp = ros::Time::now();
pub1.publish(cloud1);
pub2.publish(cloud2);
pub3.publish(cloud3);
pub4.publish(linesList);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
pcl::PointCloud<pcl::PointXYZ> OcclusionCulling::extractVisibleSurface(geometry_msgs::Pose location)
{
// >>>>>>>>>>>>>>>>>>>>
// 1. Frustum Culling
// >>>>>>>>>>>>>>>>>>>>
pcl::PointCloud <pcl::PointXYZ>::Ptr output (new pcl::PointCloud <pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr occlusionFreeCloud_local(new pcl::PointCloud<pcl::PointXYZ>);
Eigen::Matrix4f camera_pose;
Eigen::Matrix3d Rd;
Eigen::Matrix3f Rf;
camera_pose.setZero ();
// Convert quaterion orientation to XYZ angles (?)
tf::Quaternion qt;
qt.setX(location.orientation.x);
qt.setY(location.orientation.y);
qt.setZ(location.orientation.z);
qt.setW(location.orientation.w);
tf::Matrix3x3 R_tf(qt);
tf::matrixTFToEigen(R_tf,Rd);
Rf = Rd.cast<float>();
camera_pose.block (0, 0, 3, 3) = Rf;
// Set position
Eigen::Vector3f T;
T (0) = location.position.x;
T (1) = location.position.y;
T (2) = location.position.z;
camera_pose.block (0, 3, 3, 1) = T;
// Set pose
camera_pose (3, 3) = 1;
fc.setCameraPose (camera_pose);
// Perform culling
ros::Time tic = ros::Time::now();
fc.filter (*output);
ros::Time toc = ros::Time::now();
//std::cout<<"\nFrustum Filter took:"<< toc.toSec() - tic.toSec();
FrustumCloud->points= output->points;
// >>>>>>>>>>>>>>>>>>>>
// 2. Voxel grid occlusion estimation
// >>>>>>>>>>>>>>>>>>>>
Eigen::Quaternionf quat(qt.w(),qt.x(),qt.y(),qt.z());
output->sensor_origin_ = Eigen::Vector4f(T[0],T[1],T[2],0);
output->sensor_orientation_= quat;
pcl::VoxelGridOcclusionEstimationT voxelFilter;
voxelFilter.setInputCloud (output);
voxelFilter.setLeafSize (voxelRes, voxelRes, voxelRes);
voxelFilter.initializeVoxelGrid();
int state,ret;
pcl::PointXYZ pt,p1,p2;
pcl::PointXYZRGB point;
std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > out_ray;
//std::vector<geometry_msgs::Point> lineSegments;
geometry_msgs::Point linePoint;
// iterate over the entire frustum points
for ( int i = 0; i < (int)output->points.size(); i ++ )
{
// Get voxel centroid corresponding to selected point
pcl::PointXYZ ptest = output->points[i];
Eigen::Vector3i ijk = voxelFilter.getGridCoordinates( ptest.x, ptest.y, ptest.z);
if(voxelFilter.getCentroidIndexAt(ijk) == -1 ) {
// Voxel is out of bounds
continue;
}
Eigen::Vector4f centroid = voxelFilter.getCentroidCoordinate (ijk);
point = pcl::PointXYZRGB(0,244,0);
point.x = centroid[0];
point.y = centroid[1];
point.z = centroid[2];
// >>>>>>>>>>>>>>>>>>>>
// 2.1 Perform occlusion estimation
// >>>>>>>>>>>>>>>>>>>>
/*
* The way this works is it traces a line to the point and finds
* the distance to the first occupied voxel in its path (t_min).
* It then determines if the distance between the target point and
* the collided voxel are close (within voxelRes).
*
* If they are, the point is visible. Otherwise, the point is behind
* an occluding point
*
*
* This is the expected function of the following command:
* ret = voxelFilter.occlusionEstimation( state,out_ray, ijk);
*
* However, it sometimes shows occluded points on the edge of the cloud
*/
// Direction to target voxel
Eigen::Vector4f direction = centroid - output->sensor_origin_;
direction.normalize ();
// Estimate entry point into the voxel grid
float tmin = voxelFilter.rayBoxIntersection (output->sensor_origin_, direction,p1,p2); //where did this 4-input syntax come from?
if(tmin == -1){
// ray does not intersect with the bounding box
continue;
}
// Calculate coordinate of the boundary of the voxel grid
Eigen::Vector4f start = output->sensor_origin_ + tmin * direction;
// Determine distance between boundary and target voxel centroid
Eigen::Vector4f dist_vector = centroid-start;
float distance = (dist_vector).dot(dist_vector);
if (distance > voxelRes*1.414){ // voxelRes/sqrt(2)
// ray does not correspond to this point
continue;
}
// Save point
occlusionFreeCloud_local->points.push_back(ptest);
occlusionFreeCloud->points.push_back(ptest);
// >>>>>>>>>>>>>>>>>>>>
// 2.2 Save line segment for visualization
// >>>>>>>>>>>>>>>>>>>>
linePoint.x = output->sensor_origin_[0];
linePoint.y = output->sensor_origin_[1];
linePoint.z = output->sensor_origin_[2];
lineSegments.push_back(linePoint);
linePoint.x = start[0];
linePoint.y = start[1];
linePoint.z = start[2];
lineSegments.push_back(linePoint);
occupancyGrid->points.push_back(point);
}
FreeCloud.points = occlusionFreeCloud_local->points;
return FreeCloud;
}