template <typename GeneratorT> int
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::fill (int width, int height, pcl::PointCloud<pcl::PointXY>& cloud)
{
if (width < 1)
{
PCL_ERROR ("[pcl::common::CloudGenerator] Cloud width must be >= 1\n!");
return (-1);
}
if (height < 1)
{
PCL_ERROR ("[pcl::common::CloudGenerator] Cloud height must be >= 1\n!");
return (-1);
}
if (!cloud.empty ())
PCL_WARN ("[pcl::common::CloudGenerator] Cloud data will be erased with new data\n!");
cloud.width = width;
cloud.height = height;
cloud.resize (cloud.width * cloud.height);
cloud.is_dense = true;
for (pcl::PointCloud<pcl::PointXY>::iterator points_it = cloud.begin ();
points_it != cloud.end ();
++points_it)
{
points_it->x = x_generator_.run ();
points_it->y = y_generator_.run ();
}
return (0);
}
bool loadMeshFromFile(const std::string& filename, pcl::PointCloud<PointT>& pointcloud) {
std::string::size_type index = filename.rfind(".");
if (index == std::string::npos) return false;
std::string extension = filename.substr(index + 1);
if (extension == "pcd") {
if (pcl::io::loadPCDFile<PointT>(filename, pointcloud) == 0 && !pointcloud.empty()) return true;
} else {
// Supported file types: .3dc .3ds .asc .ac .bsp .dae .dw .dxf .fbx .flt .gem .geo .iv .ive .logo .lwo .lw .lws .md2 .obj .ogr .osg .pfb .ply .shp .stl .x .wrl ...
// http://trac.openscenegraph.org/projects/osg/browser/OpenSceneGraph/trunk/src/osgPlugins
// http://trac.openscenegraph.org/projects/osg/wiki/Support/UserGuides/Plugins
osg::ref_ptr<osg::Node> node(osgDB::readNodeFile(filename));
if (node) {
osg::Geode* geode = node->asGeode();
if (geode && geode->getNumDrawables() > 0) {
osg::Geometry* geometry = geode->getDrawable(0)->asGeometry();
if (geometry) {
return convertMeshToPCLPointCloud(geometry, pointcloud);
}
}
}
}
return false;
}
Eigen::Vector4f ConvexConnectedVoxels::cloudMeanNormal(
const pcl::PointCloud<pcl::Normal>::Ptr normal,
bool isnorm) {
if (normal->empty()) {
return Eigen::Vector4f(0, 0, 0, 0);
}
float x = 0.0f;
float y = 0.0f;
float z = 0.0f;
int icounter = 0;
for (int i = 0; i < normal->size(); i++) {
if ((!isnan(normal->points[i].normal_x)) &&
(!isnan(normal->points[i].normal_y)) &&
(!isnan(normal->points[i].normal_z))) {
x += normal->points[i].normal_x;
y += normal->points[i].normal_y;
z += normal->points[i].normal_z;
icounter++;
}
}
Eigen::Vector4f n_mean = Eigen::Vector4f(
x/static_cast<float>(icounter),
y/static_cast<float>(icounter),
z/static_cast<float>(icounter),
1.0f);
if (isnorm) {
n_mean.normalize();
}
return n_mean;
}
void TSDFVolumeWrapper::integrateCloud(
const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
const pcl::PointCloud<pcl::Normal>& normals,
const Eigen::Affine3d& trans) {
assert(!cloud.empty());
tsdf_volume_->integrateCloud(cloud, normals, trans);
}
cv::Mat PointCloudImageCreator::projectPointCloudToImagePlane(
const pcl::PointCloud<PointT>::Ptr cloud,
const sensor_msgs::CameraInfo::ConstPtr &camera_info,
cv::Mat &mask) {
if (cloud->empty()) {
ROS_ERROR("INPUT CLOUD EMPTY");
return cv::Mat();
}
cv::Mat objectPoints = cv::Mat(static_cast<int>(cloud->size()), 3, CV_32F);
for (int i = 0; i < cloud->size(); i++) {
objectPoints.at<float>(i, 0) = cloud->points[i].x;
objectPoints.at<float>(i, 1) = cloud->points[i].y;
objectPoints.at<float>(i, 2) = cloud->points[i].z;
}
float K[9];
float R[9];
for (int i = 0; i < 9; i++) {
K[i] = camera_info->K[i];
R[i] = camera_info->R[i];
}
cv::Mat cameraMatrix = cv::Mat(3, 3, CV_32F, K);
cv::Mat rotationMatrix = cv::Mat(3, 3, CV_32F, R);
float tvec[3];
tvec[0] = camera_info->P[3];
tvec[1] = camera_info->P[7];
tvec[2] = camera_info->P[11];
cv::Mat translationMatrix = cv::Mat(3, 1, CV_32F, tvec);
float D[5];
for (int i = 0; i < 5; i++) {
D[i] = camera_info->D[i];
}
cv::Mat distortionModel = cv::Mat(5, 1, CV_32F, D);
cv::Mat rvec;
cv::Rodrigues(rotationMatrix, rvec);
std::vector<cv::Point2f> imagePoints;
cv::projectPoints(objectPoints, rvec, translationMatrix,
cameraMatrix, distortionModel, imagePoints);
cv::Scalar color = cv::Scalar(0, 0, 0);
cv::Mat image = cv::Mat(
camera_info->height, camera_info->width, CV_8UC3, color);
mask = cv::Mat::zeros(
camera_info->height, camera_info->width, CV_32F);
for (int i = 0; i < imagePoints.size(); i++) {
int x = imagePoints[i].x;
int y = imagePoints[i].y;
if (!std::isnan(x) && !std::isnan(y) && (x >= 0 && x <= image.cols) &&
(y >= 0 && y <= image.rows)) {
image.at<cv::Vec3b>(y, x)[2] = cloud->points[i].r;
image.at<cv::Vec3b>(y, x)[1] = cloud->points[i].g;
image.at<cv::Vec3b>(y, x)[0] = cloud->points[i].b;
mask.at<float>(y, x) = 255.0f;
}
}
return image;
}
bool calibrate(const std::string frame_id)
{
physical_points_.empty();
physical_points_.header.frame_id = fixed_frame;
cout << "Is the checkerboard correct? " << endl;
cout << "Move edge of gripper to point 1 in image and press Enter. " << endl;
cin.ignore();
addPhysicalPoint();
cout << "Move edge of gripper to point 2 in image and press Enter. " << endl;
cin.ignore();
addPhysicalPoint();
cout << "Move edge of gripper to point 3 in image and press Enter. " << endl;
cin.ignore();
addPhysicalPoint();
cout << "Move edge of gripper to point 4 in image and press Enter. " << endl;
cin.ignore();
addPhysicalPoint();
Eigen::Matrix4f t;
physical_pub_.publish(physical_points_);
// Old Method:
// pcl::estimateRigidTransformationSVD( physical_points_, image_points_, t );
pcl::registration::TransformationEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr
transformation_estimation (new pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ>);
transformation_estimation->estimateRigidTransformation ( physical_points_, image_points_, t );
// Output
tf::Transform transform = tfFromEigen(t), trans_full, camera_transform_unstamped;
tf::StampedTransform camera_transform;
cout << "Resulting transform (camera frame -> fixed frame): " << endl << t << endl << endl;
try
{
tf_listener_.lookupTransform(frame_id, camera_frame, ros::Time(0), camera_transform);
}
catch (tf::TransformException& ex)
{
ROS_WARN("[calibrate] TF exception:\n%s", ex.what());
return false;
}
camera_transform_unstamped = camera_transform;
trans_full = camera_transform_unstamped.inverse()*transform;
Eigen::Matrix4f t_full = EigenFromTF(trans_full);
Eigen::Matrix4f t_full_inv = (Eigen::Transform<float,3,Affine>(t_full).inverse()).matrix();
cout << "Resulting transform (fixed frame -> camera frame): " << endl << t_full << endl << endl;
printStaticTransform(t_full_inv, fixed_frame, camera_frame);
return true;
}
void viz_cb (pcl::visualization::PCLVisualizer& viz)
{
// cout << "PbMapMaker::viz_cb(...)\n";
if (cloud1->empty())
{
boost::this_thread::sleep (boost::posix_time::milliseconds (10));
return;
}
{
// boost::mutex::scoped_lock lock (viz_mutex);
// viz.removeAllShapes();
viz.removeAllPointClouds();
{ //mrpt::synch::CCriticalSectionLocker csl(&CS_visualize);
boost::mutex::scoped_lock updateLock(visualizationMutex);
// if (!viz.updatePointCloud (cloud, "sphereCloud"))
// viz.addPointCloud (sphereCloud, "sphereCloud");
if (!viz.updatePointCloud (cloud1, "cloud1"))
viz.addPointCloud (cloud1, "cloud1");
if (!viz.updatePointCloud (cloud2, "cloud2"))
viz.addPointCloud (cloud2, "cloud2");
if (!viz.updatePointCloud (cloud3, "cloud3"))
viz.addPointCloud (cloud3, "cloud3");
if (!viz.updatePointCloud (cloud4, "cloud4"))
viz.addPointCloud (cloud4, "cloud4");
if (!viz.updatePointCloud (cloud5, "cloud5"))
viz.addPointCloud (cloud5, "cloud5");
if (!viz.updatePointCloud (cloud6, "cloud6"))
viz.addPointCloud (cloud6, "cloud6");
if (!viz.updatePointCloud (cloud7, "cloud7"))
viz.addPointCloud (cloud7, "cloud7");
if (!viz.updatePointCloud (cloud8, "cloud8"))
viz.addPointCloud (cloud8, "cloud8");
updateLock.unlock();
}
}
}
bool insertPoints(const osg::Geometry* geometry, pcl::PointCloud<PointT>& pointcloud) {
const osg::Vec3Array* vertex_points = (osg::Vec3Array*)geometry->getVertexArray();
for (osg::Vec3Array::size_type i = 0; i < vertex_points->size(); ++i) {
PointT point;
point.x = (*vertex_points)[i][0];
point.y = (*vertex_points)[i][1];
point.z = (*vertex_points)[i][2];
pointcloud.push_back(point);
}
pointcloud.is_dense = false;
pointcloud.width = pointcloud.size();
pointcloud.height = 1;
return !pointcloud.empty();
}
bool addNormals(const osg::Geometry* geometry, pcl::PointCloud<PointT>& pointcloud) {
const osg::Vec3Array* vertex_normals = (osg::Vec3Array*)geometry->getNormalArray();
size_t pointcloud_index = 0;
size_t pointcloud_normal_stride = (geometry->getNormalBinding() != osg::Geometry::BIND_PER_VERTEX) ? 3 : 1;
for (osg::Vec3Array::size_type normals_index = 0; normals_index < vertex_normals->size(); ++normals_index) {
for (size_t i = 0; i < pointcloud_normal_stride && pointcloud_index < pointcloud.size(); ++i) {
PointT* point = &pointcloud.points[pointcloud_index++];
point->normal_x = (*vertex_normals)[normals_index][0];
point->normal_y = (*vertex_normals)[normals_index][1];
point->normal_z = (*vertex_normals)[normals_index][2];
}
}
return !pointcloud.empty();
}
bool addRGB(const osg::Geometry* geometry, pcl::PointCloud<PointT>& pointcloud) {
const osg::Vec4Array* vertex_colors = (osg::Vec4Array*) geometry->getColorArray();
size_t pointcloud_index = 0;
size_t pointcloud_color_stride = (geometry->getColorBinding() != osg::Geometry::BIND_PER_VERTEX) ? 3 : 1;
for (osg::Vec4Array::size_type colors_index = 0; colors_index < vertex_colors->size(); ++colors_index) {
for (size_t i = 0; i < pointcloud_color_stride && pointcloud_index < pointcloud.size(); ++i) {
PointT* point = &pointcloud.points[pointcloud_index++];
point->r = (*vertex_colors)[colors_index][0] * 256;
point->g = (*vertex_colors)[colors_index][1] * 256;
point->b = (*vertex_colors)[colors_index][2] * 256;
}
}
return !pointcloud.empty();
}
int Conversion::convert(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr & colorCloud,
const int & r, const int & g, const int & b){
colorCloud->empty();
for (int i=0; i<cloud->points.size ();i++){
pcl::PointXYZRGB point;
point.x = cloud->points[i].x;
point.y = cloud->points[i].y;
point.z = cloud->points[i].z;
point.r = r;
point.g = g;
point.b = b;
colorCloud->push_back(point);
}
return 1;
}
template <typename PointT, typename Scalar> inline unsigned int
pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
Eigen::Matrix<Scalar, 4, 1> ¢roid)
{
if (cloud.empty ())
return (0);
// Initialize to 0
centroid.setZero ();
// For each point in the cloud
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
for (size_t i = 0; i < cloud.size (); ++i)
{
centroid[0] += cloud[i].x;
centroid[1] += cloud[i].y;
centroid[2] += cloud[i].z;
}
centroid[3] = 0;
centroid /= static_cast<Scalar> (cloud.size ());
return (static_cast<unsigned int> (cloud.size ()));
}
// NaN or Inf values could exist => check for them
else
{
unsigned cp = 0;
for (size_t i = 0; i < cloud.size (); ++i)
{
// Check if the point is invalid
if (!isFinite (cloud [i]))
continue;
centroid[0] += cloud[i].x;
centroid[1] += cloud[i].y;
centroid[2] += cloud[i].z;
++cp;
}
centroid[3] = 0;
centroid /= static_cast<Scalar> (cp);
return (cp);
}
}
template <typename PointT, typename Scalar> inline void
pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
{
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Get the size of the fields
centroid.setZero (boost::mpl::size<FieldList>::value);
if (cloud.empty ())
return;
// Iterate over each point
int size = static_cast<int> (cloud.size ());
for (int i = 0; i < size; ++i)
{
// Iterate over each dimension
pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[i], centroid));
}
centroid /= static_cast<Scalar> (size);
}
/* ------------------------------------------------------------------------------------------ */
void makeImage () {
// Compute the maximum depth
double maxDepth = -1, minDepth = 10000000;
for (int h=0; h<point_cloud_ptr->height; h++) {
for (int w=0; w<point_cloud_ptr->width; w++) {
pcl::PointXYZRGBA point = point_cloud_ptr->at(w, h);
if(point.z > maxDepth) maxDepth = point.z;
if(point.z < minDepth) minDepth = point.z;
}
}
// Create the image
im = cv::Mat(point_cloud_ptr->height, 2*point_cloud_ptr->width, CV_8UC3);
if (point_cloud_ptr->empty()) return;
for (int h=0; h<im.rows; h++) {
for (int w=0; w<point_cloud_ptr->width; w++) {
pcl::PointXYZRGBA point = point_cloud_ptr->at(w, h);
Eigen::Vector3i rgb = point.getRGBVector3i();
im.at<cv::Vec3b>(h,w)[0] = rgb[2];
im.at<cv::Vec3b>(h,w)[1] = rgb[1];
im.at<cv::Vec3b>(h,w)[2] = rgb[0];
}
for (int w=0; w<point_cloud_ptr->width; w++) {
pcl::PointXYZRGBA point = point_cloud_ptr->at(w, h);
int val = (int) (255 * ((point.z-minDepth)/(maxDepth-minDepth)));
im.at<cv::Vec3b>(h,w+point_cloud_ptr->width)[0] = val;
im.at<cv::Vec3b>(h,w+point_cloud_ptr->width)[1] = val;
im.at<cv::Vec3b>(h,w+point_cloud_ptr->width)[2] = val;
}
}
// Convert the image to hsv for processing later
cv::cvtColor(im, im_hsv, cv::COLOR_BGR2HSV);
cv::imshow("image", im);
cv::waitKey(1);
}
static int vscanDetection(int closest_waypoint)
{
if (_vscan.empty() == true)
return -1;
for (int i = closest_waypoint + 1; i < closest_waypoint + _search_distance; i++) {
if(i > _path_dk.getPathSize() - 1 )
return -1;
tf::Vector3 tf_waypoint = _path_dk.transformWaypoint(i);
//tf::Vector3 tf_waypoint = TransformWaypoint(_transform,_current_path.waypoints[i].pose.pose);
tf_waypoint.setZ(0);
//std::cout << "waypoint : "<< tf_waypoint.getX() << " "<< tf_waypoint.getY() << std::endl;
int point_count = 0;
for (pcl::PointCloud<pcl::PointXYZ>::const_iterator item = _vscan.begin(); item != _vscan.end(); item++) {
if ((item->x == 0 && item->y == 0) || item->z > _detection_height_top || item->z < _detection_height_bottom)
continue;
tf::Vector3 point((double) item->x, (double) item->y, 0);
double dt = tf::tfDistance(point, tf_waypoint);
if (dt < _detection_range) {
point_count++;
//std::cout << "distance :" << dt << std::endl;
//std::cout << "point : "<< (double) item->x << " " << (double)item->y << " " <<(double) item->z << std::endl;
//std::cout << "count : "<< point_count << std::endl;
}
if (point_count > _threshold_points)
return i;
}
}
return -1;
}
int extremeFinder::find(pcl::PointCloud<PointT>::Ptr cloud)
{
if (cloud->empty() || cloud->size() == 0)
{
std::cout << "Cloud empty!" << std::endl;
return -1;
}
for (int i = 0; i < cloud->size(); i++)
{
try {
PointT p = cloud->at(i);
if (p.x > x_max) x_max = p.x;
if (p.x < x_min) x_min = p.x;
if (p.y > y_max) y_max = p.y;
if (p.y < y_min) y_min = p.y;
if (p.z > z_max) z_max = p.z;
if (p.z < z_min) z_min = p.z;
x_mean += p.x;
y_mean += p.y;
z_mean += p.z;
}
catch (const std::out_of_range& err) {
std::cerr << "Out of Range error: " << err.what()
<< ". Iteration number: " << i << std::endl;
}
}
x_mean /= cloud->size();
y_mean /= cloud->size();
z_mean /= cloud->size();
return 0;
}
bool updateViewer(const pcl::PointCloud<pcl::PointXYZRGB> &pointCloud, const Eigen::Matrix4f &pose)
{
if (!initialized) { return true; }
if (viewer->wasStopped()) { return false; }
cloudMutex.lock();
if (cloudInit->empty())
{
pcl::copyPointCloud(pointCloud, *cloudInit);
cloudInitUpdated = true;
}
else
{
pcl::copyPointCloud(pointCloud, *cloud);
cloudPose = pose;
cloudUpdated = true;
}
cloudMutex.unlock();
// while(pausing) { boost::this_thread::sleep(boost::posix_time::milliseconds(1)); }
pausing = true;
return !viewer->wasStopped();
}
void createPC() {
if(!cloud->empty())
cloud->clear();
for( int y=0; y<480; y++) {
for( int x=0; x<640; x++) {
Scalar intensity = fgMaskMOG.at<uchar>(y,x);
if (intensity.val[0]==255) {
float d=(float)depth_image.at<ushort>(y,x);
Vec3b color = rgb_image.at<Vec3b>(y,x);
Vector4f temp;
temp << x*d,y*d,d,1.0;
Vector4f point = t_mat*temp;
pcl::PointXYZRGBA result;
result.x = point(0);
result.y = point(1);
result.z = point(2);
result.r = color.val[2];
result.g = color.val[1];
result.b = color.val[0];
cloud->points.push_back(result);
}
}
}
}
template <typename PointT, typename Scalar> inline unsigned
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const Eigen::Matrix<Scalar, 4, 1> ¢roid,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
if (cloud.empty ())
return (0);
// Initialize to 0
covariance_matrix.setZero ();
unsigned point_count;
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
point_count = static_cast<unsigned> (cloud.size ());
// For each point in the cloud
for (size_t i = 0; i < point_count; ++i)
{
Eigen::Matrix<Scalar, 4, 1> pt;
pt[0] = cloud[i].x - centroid[0];
pt[1] = cloud[i].y - centroid[1];
pt[2] = cloud[i].z - centroid[2];
covariance_matrix (1, 1) += pt.y () * pt.y ();
covariance_matrix (1, 2) += pt.y () * pt.z ();
covariance_matrix (2, 2) += pt.z () * pt.z ();
pt *= pt.x ();
covariance_matrix (0, 0) += pt.x ();
covariance_matrix (0, 1) += pt.y ();
covariance_matrix (0, 2) += pt.z ();
}
}
// NaN or Inf values could exist => check for them
else
{
point_count = 0;
// For each point in the cloud
for (size_t i = 0; i < cloud.size (); ++i)
{
// Check if the point is invalid
if (!isFinite (cloud [i]))
continue;
Eigen::Matrix<Scalar, 4, 1> pt;
pt[0] = cloud[i].x - centroid[0];
pt[1] = cloud[i].y - centroid[1];
pt[2] = cloud[i].z - centroid[2];
covariance_matrix (1, 1) += pt.y () * pt.y ();
covariance_matrix (1, 2) += pt.y () * pt.z ();
covariance_matrix (2, 2) += pt.z () * pt.z ();
pt *= pt.x ();
covariance_matrix (0, 0) += pt.x ();
covariance_matrix (0, 1) += pt.y ();
covariance_matrix (0, 2) += pt.z ();
++point_count;
}
}
covariance_matrix (1, 0) = covariance_matrix (0, 1);
covariance_matrix (2, 0) = covariance_matrix (0, 2);
covariance_matrix (2, 1) = covariance_matrix (1, 2);
return (point_count);
}
void ConvexConnectedVoxels::surfelLevelObjectHypothesis(
pcl::PointCloud<PointT>::Ptr cloud,
pcl::PointCloud<pcl::Normal>::Ptr normals,
std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr > &convex_supervoxels) {
if (cloud->empty()) {
ROS_ERROR("ERROR: EMPTY CLOUD");
return;
}
std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters;
AdjacencyList adjacency_list;
this->supervoxelSegmentation(cloud,
supervoxel_clusters,
adjacency_list);
std::map<uint32_t, int> voxel_labels;
convex_supervoxels.clear();
// cloud->clear();
for (std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr>::iterator it =
supervoxel_clusters.begin(); it != supervoxel_clusters.end();
it++) {
voxel_labels[it->first] = -1;
// pcl::Supervoxel<PointT>::Ptr supervoxel =
// supervoxel_clusters.at(it->first);
}
int label = -1;
AdjacencyList::vertex_iterator i, end;
/*
std::cout << supervoxel_clusters.size() << "\n\n";
for (boost::tie(i, end) = boost::vertices(adjacency_list); i != end; i++) {
AdjacencyList::adjacency_iterator ai, a_end;
boost::tie(ai, a_end) = boost::adjacent_vertices(*i, adjacency_list);
std::cout << adjacency_list[*i] << "\t ->";
for (; ai != a_end; ai++) {
std::cout << adjacency_list[*ai] << ", ";
}
std::cout << "\n";
}
ROS_WARN("DONE\n\n");
return;
*/
std::vector<uint32_t> good_vertices;
for (boost::tie(i, end) = boost::vertices(adjacency_list); i != end; i++) {
AdjacencyList::adjacency_iterator ai, a_end;
boost::tie(ai, a_end) = boost::adjacent_vertices(*i, adjacency_list);
uint32_t vindex = static_cast<int>(adjacency_list[*i]);
// Eigen::Vector4f v_normal = this->cloudMeanNormal(
// supervoxel_clusters.at(vindex)->normals_);
Eigen::Vector4f v_normal = supervoxel_clusters.at(
vindex)->normal_.getNormalVector4fMap();
std::map<uint32_t, int>::iterator it = voxel_labels.find(vindex);
if (it->second == -1) {
voxel_labels[vindex] = ++label;
}
bool vertex_has_neigbor = true;
if (ai == a_end) {
vertex_has_neigbor = false;
std::cout << "SKIP CHECKING : "<< vindex << "\t"
<< vertex_has_neigbor << "\n";
if (!supervoxel_clusters.at(vindex)->voxels_->empty()) {
convex_supervoxels[vindex] = supervoxel_clusters.at(vindex);
}
}
std::vector<uint32_t> neigb_ind;
while (vertex_has_neigbor) {
bool found = false;
AdjacencyList::edge_descriptor e_descriptor;
boost::tie(e_descriptor, found) = boost::edge(
*i, *ai, adjacency_list);
if (found) {
float weight = adjacency_list[e_descriptor];
uint32_t n_vindex = adjacency_list[*ai];
std::cout << "processing: " << n_vindex << "\t"
<< supervoxel_clusters.size() << "\n";
Eigen::Vector4f update_normal = this->cloudMeanNormal(
supervoxel_clusters.at(vindex)->normals_, true);
Eigen::Vector4f update_centroid;
pcl::compute3DCentroid<PointT, float>(
*(supervoxel_clusters.at(vindex)->voxels_), update_centroid);
Eigen::Vector4f dist = (
supervoxel_clusters.at(vindex)->centroid_.getVector4fMap() -
supervoxel_clusters.at(n_vindex)->centroid_.getVector4fMap())/
(supervoxel_clusters.at(vindex)->centroid_.getVector4fMap() -
supervoxel_clusters.at(n_vindex)->centroid_.getVector4fMap()
).norm();
/*
Eigen::Vector4f dist = (
update_centroid - supervoxel_clusters.at(
n_vindex)->centroid_.getVector4fMap()) / (
update_centroid - supervoxel_clusters.at(
n_vindex)->centroid_.getVector4fMap()).norm();
*/
float conv_criteria = (
// this->cloudMeanNormal(supervoxel_clusters.at(vindex)->normals_) -
// supervoxel_clusters.at(vindex)->normal_.getNormalVector4fMap()-
update_normal -
supervoxel_clusters.at(n_vindex)->normal_.getNormalVector4fMap()
).dot(dist);
// conv_criteria = (supervoxel_clusters.at(
// n_vindex)->centroid_.getVector4fMap() -
// update_centroid).dot(supervoxel_clusters.at(
// n_vindex)->normal_.getNormalVector4fMap());
neigb_ind.push_back(n_vindex);
if (conv_criteria <= static_cast<float>(this->convex_threshold_) ||
isnan(conv_criteria)) {
boost::remove_edge(e_descriptor, adjacency_list);
} else {
this->updateSupervoxelClusters(supervoxel_clusters,
vindex, n_vindex);
AdjacencyList::adjacency_iterator ni, n_end;
boost::tie(ni, n_end) = boost::adjacent_vertices(
*ai, adjacency_list);
for (; ni != n_end; ni++) {
bool is_found = false;
AdjacencyList::edge_descriptor n_edge;
boost::tie(n_edge, is_found) = boost::edge(
*ai, *ni, adjacency_list);
if (is_found && (*ni != *i)) {
boost::add_edge(*i, *ni, FLT_MIN, adjacency_list);
// boost::remove_edge(n_edge, adjacency_list);
} else if (is_found && (*ni == *i)) {
// boost::remove_edge(n_edge, adjacency_list);
}
std::cout << "\t\tREMVING: " << adjacency_list[*ni] << "\t"
<< n_vindex << "\t" << vindex << "\t"
<< adjacency_list[*ai] << "\n";
}
ROS_WARN("REMOVING");
boost::clear_vertex(*ai, adjacency_list);
// std::cout << adjacency_list[boost::source(
// e_descriptor, adjacency_list)] << "\n";
// std::cout << adjacency_list[boost::target(
// e_descriptor, adjacency_list)] << "\n";
// boost::clear_vertex(v1, adjacency_list);
ROS_WARN("ADDING");
voxel_labels[n_vindex] = label;
}
}
ROS_INFO("UPDATING");
boost::tie(ai, a_end) = boost::adjacent_vertices(*i, adjacency_list);
if (ai == a_end) {
convex_supervoxels[vindex] = supervoxel_clusters.at(vindex);
vertex_has_neigbor = false;
} else {
vertex_has_neigbor = true;
}
}
if (ai == a_end) {
// convex_supervoxels[vindex] = supervoxel_clusters.at(vindex);
}
}
// convex_supervoxels.clear();
// std::vector<bool> flag;
// for (int i = 0; i < label; i++) {
// flag.push_back(false);
// }
// for (std::map<uint32_t, int>::iterator it = voxel_labels.begin();
// it != voxel_labels.end(); it++) {
// if (!flag[i])
// convex_supervoxels[]
// std::cout << it->first << ", " << it->second << "\n";
// }
std::cout << "\033[31m LABEL: \033[0m" << label << "\n";
std::cout << convex_supervoxels.size() << "\t" << good_vertices.size()
<< "\n\n";
supervoxel_clusters.clear();
}
static EControl vscanDetection()
{
if (_vscan.empty() == true || _closest_waypoint < 0)
return KEEP;
int decelerate_or_stop = -10000;
int decelerate2stop_waypoints = 15;
for (int i = _closest_waypoint; i < _closest_waypoint + _search_distance; i++) {
g_obstacle.clearStopPoints();
if (!g_obstacle.isDecided())
g_obstacle.clearDeceleratePoints();
decelerate_or_stop++;
if (decelerate_or_stop > decelerate2stop_waypoints ||
(decelerate_or_stop >= 0 && i >= _path_dk.getSize()-1) ||
(decelerate_or_stop >= 0 && i == _closest_waypoint+_search_distance-1))
return DECELERATE;
if (i > _path_dk.getSize() - 1 )
return KEEP;
// Detection for cross walk
if (i == vmap.getDetectionWaypoint()) {
if (CrossWalkDetection(vmap.getDetectionCrossWalkID()) == STOP) {
_obstacle_waypoint = i;
return STOP;
}
}
// waypoint seen by vehicle
geometry_msgs::Point waypoint = calcRelativeCoordinate(_path_dk.getWaypointPosition(i),
_current_pose.pose);
tf::Vector3 tf_waypoint = point2vector(waypoint);
tf_waypoint.setZ(0);
int stop_point_count = 0;
int decelerate_point_count = 0;
for (pcl::PointCloud<pcl::PointXYZ>::const_iterator item = _vscan.begin(); item != _vscan.end(); item++) {
tf::Vector3 vscan_vector((double) item->x, (double) item->y, 0);
// for simulation
if (g_sim_mode) {
tf::Transform transform;
tf::poseMsgToTF(_sim_ndt_pose.pose, transform);
geometry_msgs::Point world2vscan = vector2point(transform * vscan_vector);
vscan_vector = point2vector(calcRelativeCoordinate(world2vscan, _current_pose.pose));
vscan_vector.setZ(0);
}
// 2D distance between waypoint and vscan points(obstacle)
// ---STOP OBSTACLE DETECTION---
double dt = tf::tfDistance(vscan_vector, tf_waypoint);
if (dt < _detection_range) {
stop_point_count++;
geometry_msgs::Point vscan_temp;
vscan_temp.x = item->x;
vscan_temp.y = item->y;
vscan_temp.z = item->z;
if (g_sim_mode)
g_obstacle.setStopPoint(calcAbsoluteCoordinate(vscan_temp, _sim_ndt_pose.pose));
else
g_obstacle.setStopPoint(calcAbsoluteCoordinate(vscan_temp, _current_pose.pose));
}
if (stop_point_count > _threshold_points) {
_obstacle_waypoint = i;
return STOP;
}
// without deceleration range
if (_deceleration_range < 0.01)
continue;
// deceleration search runs "decelerate_search_distance" waypoints from closest
if (i > _closest_waypoint+_deceleration_search_distance || decelerate_or_stop >= 0)
continue;
// ---DECELERATE OBSTACLE DETECTION---
if (dt > _detection_range && dt < _detection_range + _deceleration_range) {
bool count_flag = true;
// search overlaps between DETECTION range and DECELERATION range
for (int waypoint_search = -5; waypoint_search <= 5; waypoint_search++) {
if (i+waypoint_search < 0 || i+waypoint_search >= _path_dk.getSize() || !waypoint_search)
continue;
geometry_msgs::Point temp_waypoint = calcRelativeCoordinate(
_path_dk.getWaypointPosition(i+waypoint_search),
_current_pose.pose);
tf::Vector3 waypoint_vector = point2vector(temp_waypoint);
waypoint_vector.setZ(0);
// if there is a overlap, give priority to DETECTION range
if (tf::tfDistance(vscan_vector, waypoint_vector) < _detection_range) {
count_flag = false;
break;
}
}
if (count_flag) {
decelerate_point_count++;
geometry_msgs::Point vscan_temp;
vscan_temp.x = item->x;
vscan_temp.y = item->y;
vscan_temp.z = item->z;
if (g_sim_mode)
g_obstacle.setDeceleratePoint(calcAbsoluteCoordinate(vscan_temp, _sim_ndt_pose.pose));
else
g_obstacle.setDeceleratePoint(calcAbsoluteCoordinate(vscan_temp, _current_pose.pose));
}
}
// found obstacle to DECELERATE
if (decelerate_point_count > _threshold_points) {
_obstacle_waypoint = i;
decelerate_or_stop = 0; // for searching near STOP obstacle
g_obstacle.setDecided(true);
}
}
}
return KEEP; //no obstacles
}
cv::Mat PointCloudImageCreator::projectPointCloudToImagePlane(
const pcl::PointCloud<PointT>::Ptr cloud,
const jsk_recognition_msgs::ClusterPointIndices::ConstPtr indices,
const sensor_msgs::CameraInfo::ConstPtr &camera_info,
cv::Mat &mask) {
if (cloud->empty()) {
ROS_ERROR("INPUT CLOUD EMPTY");
return cv::Mat();
}
cv::Mat objectPoints = cv::Mat(static_cast<int>(cloud->size()), 3, CV_32F);
cv::RNG rng(12345);
std::vector<cv::Vec3b> colors;
std::vector<int> labels(cloud->size());
for (int j = 0; j < indices->cluster_indices.size(); j++) {
std::vector<int> point_indices = indices->cluster_indices[j].indices;
for (auto it = point_indices.begin(); it != point_indices.end(); it++) {
int i = *it;
objectPoints.at<float>(i, 0) = cloud->points[i].x;
objectPoints.at<float>(i, 1) = cloud->points[i].y;
objectPoints.at<float>(i, 2) = cloud->points[i].z;
labels[i] = j;
}
colors.push_back(cv::Vec3b(rng.uniform(0, 255),
rng.uniform(0, 255),
rng.uniform(0, 255)));
}
float K[9];
float R[9];
for (int i = 0; i < 9; i++) {
K[i] = camera_info->K[i];
R[i] = camera_info->R[i];
}
cv::Mat cameraMatrix = cv::Mat(3, 3, CV_32F, K);
cv::Mat rotationMatrix = cv::Mat(3, 3, CV_32F, R);
float tvec[3];
tvec[0] = camera_info->P[3];
tvec[1] = camera_info->P[7];
tvec[2] = camera_info->P[11];
cv::Mat translationMatrix = cv::Mat(3, 1, CV_32F, tvec);
float D[5];
for (int i = 0; i < 5; i++) {
D[i] = camera_info->D[i];
}
cv::Mat distortionModel = cv::Mat(5, 1, CV_32F, D);
cv::Mat rvec;
cv::Rodrigues(rotationMatrix, rvec);
std::vector<cv::Point2f> imagePoints;
cv::projectPoints(objectPoints, rvec, translationMatrix,
cameraMatrix, distortionModel, imagePoints);
cv::Scalar color = cv::Scalar(0, 0, 0);
cv::Mat image = cv::Mat(
camera_info->height, camera_info->width, CV_8UC3, color);
mask = cv::Mat::zeros(
camera_info->height, camera_info->width, CV_32F);
for (int i = 0; i < imagePoints.size(); i++) {
int x = imagePoints[i].x;
int y = imagePoints[i].y;
if (!std::isnan(x) && !std::isnan(y) && (x >= 0 && x <= image.cols) &&
(y >= 0 && y <= image.rows)) {
image.at<cv::Vec3b>(y, x)[2] = labels[i] + 1;
image.at<cv::Vec3b>(y, x)[1] = labels[i] + 1;
image.at<cv::Vec3b>(y, x)[0] = labels[i] + 1;
mask.at<float>(y, x) = 255.0f;
}
}
return image;
}
bool convertMeshToPCLPointCloud(const osg::Geometry* geometry, pcl::PointCloud<PointT>& pointcloud) {
insertPoints(geometry, pointcloud);
return !pointcloud.empty();
}
// global variables used: NONE
void
vtree_user::compute_features( pcl::PointCloud<PointT>::Ptr & point_cloud,
pcl::PointCloud<PointT>::Ptr & keypoints,
pcl::PointCloud<FeatureType>::Ptr & features )
{
// Define Parameters
// IMPORTANT: the radius used for features has to be larger than the radius used to estimate the surface normals!!!
#if FEATURE == 1 // FPFH
float keypoint_radius_(0.032f);
float normal_radius_(0.04f);
float feature_radius_(0.063f);
#elif FEATURE == 2 // PFH
float keypoint_radius_(0.02f);
float normal_radius_(0.03f);
float feature_radius_(0.04f);
#elif FEATURE == 3 // VFH
float keypoint_radius_(0.02f);
float normal_radius_(0.03f);
float feature_radius_(0.04f);
#endif
ROS_INFO("[vtree_user] Starting keypoint extraction...");
clock_t tic = clock();
pcl::PointCloud<int>::Ptr keypoint_idx(new pcl::PointCloud<int>);
extract_keypoints( point_cloud, keypoints, keypoint_idx, keypoint_radius_ );
ROS_INFO("[vtree_user] Keypoint extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
if( keypoints->empty() )
{
ROS_WARN("[vtree_user] No keypoints were found...");
return;
}
// Compute normals for the input cloud
ROS_INFO("[vtree_user] Starting normal extraction...");
tic = clock();
pcl::PointCloud<pcl::Normal>::Ptr normals_(new pcl::PointCloud<pcl::Normal>);
SearchMethod::Ptr search_method_xyz_ (new SearchMethod);
pcl::NormalEstimation<PointT, pcl::Normal> norm_est;
norm_est.setInputCloud ( point_cloud );
norm_est.setSearchMethod (search_method_xyz_);
norm_est.setRadiusSearch (normal_radius_);
norm_est.compute (*normals_);
ROS_INFO("[vtree_user] Normal extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
// Get features at the computed keypoints
ROS_INFO("[vtree_user] Starting feature computation...");
boost::shared_ptr<std::vector<int> > key_idx_ptr( new std::vector<int>);
for( pcl::PointCloud<int>::iterator it = keypoint_idx->begin();
it != keypoint_idx->end(); ++it)
key_idx_ptr->push_back( *it );
tic = clock();
FeatureEst feat_est;
feat_est.setInputCloud ( point_cloud );
feat_est.setInputNormals (normals_);
feat_est.setIndices( key_idx_ptr );
search_method_xyz_.reset(new SearchMethod);
feat_est.setSearchMethod (search_method_xyz_);
feat_est.setRadiusSearch (feature_radius_);
feat_est.compute ( *features );
/*
feat_est.setSearchSurface (point_cloud);
feat_est.setInputNormals (normals_);
feat_est.setInputCloud ( keypoints );
search_method_xyz_.reset(new SearchMethod);
feat_est.setSearchMethod (search_method_xyz_);
feat_est.setRadiusSearch (feature_radius_);
feat_est.compute ( *features );
*/
ROS_INFO("[vtree_user] Feature computation took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
}
template <typename PointT> int
pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PointCloud<PointT> &cloud,
const int precision)
{
if (cloud.empty ())
{
throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data!");
return (-1);
}
if (cloud.width * cloud.height != cloud.points.size ())
{
throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
return (-1);
}
std::ofstream fs;
fs.open (file_name.c_str ()); // Open file
if (!fs.is_open () || fs.fail ())
{
throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!");
return (-1);
}
fs.precision (precision);
fs.imbue (std::locale::classic ());
std::vector<sensor_msgs::PointField> fields;
pcl::getFields (cloud, fields);
// Write the header information
fs << generateHeader<PointT> (cloud) << "DATA ascii\n";
std::ostringstream stream;
stream.precision (precision);
stream.imbue (std::locale::classic ());
// Iterate through the points
for (size_t i = 0; i < cloud.points.size (); ++i)
{
for (size_t d = 0; d < fields.size (); ++d)
{
// Ignore invalid padded dimensions that are inherited from binary data
if (fields[d].name == "_")
continue;
int count = fields[d].count;
if (count == 0)
count = 1; // we simply cannot tolerate 0 counts (coming from older converter code)
for (int c = 0; c < count; ++c)
{
switch (fields[d].datatype)
{
case sensor_msgs::PointField::INT8:
{
int8_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (int8_t), sizeof (int8_t));
//if (pcl_isnan (value))
// stream << "nan";
//else
stream << boost::numeric_cast<uint32_t>(value);
break;
}
case sensor_msgs::PointField::UINT8:
{
uint8_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (uint8_t), sizeof (uint8_t));
//if (pcl_isnan (value))
// stream << "nan";
//else
stream << boost::numeric_cast<uint32_t>(value);
break;
}
case sensor_msgs::PointField::INT16:
{
int16_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (int16_t), sizeof (int16_t));
//if (pcl_isnan (value))
// stream << "nan";
//else
stream << boost::numeric_cast<int16_t>(value);
break;
}
case sensor_msgs::PointField::UINT16:
{
uint16_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (uint16_t), sizeof (uint16_t));
//if (pcl_isnan (value))
// stream << "nan";
//else
stream << boost::numeric_cast<uint16_t>(value);
break;
}
case sensor_msgs::PointField::INT32:
{
int32_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (int32_t), sizeof (int32_t));
//if (pcl_isnan (value))
// stream << "nan";
//else
stream << boost::numeric_cast<int32_t>(value);
break;
}
case sensor_msgs::PointField::UINT32:
{
uint32_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (uint32_t), sizeof (uint32_t));
//if (pcl_isnan (value))
// stream << "nan";
//else
stream << boost::numeric_cast<uint32_t>(value);
break;
}
case sensor_msgs::PointField::FLOAT32:
{
float value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (float), sizeof (float));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<float>(value);
break;
}
case sensor_msgs::PointField::FLOAT64:
{
double value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[i]) + fields[d].offset + c * sizeof (double), sizeof (double));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<double>(value);
break;
}
default:
PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
break;
}
if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
stream << " ";
}
}
// Copy the stream, trim it, and write it to disk
std::string result = stream.str ();
boost::trim (result);
stream.str ("");
fs << result << "\n";
}
fs.close (); // Close file
return (0);
}
void OpenniFilter::cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
{
if (!viewer.wasStopped())
{
if (cloud->isOrganized())
{
// initialize all the Mats to store intermediate steps
int cloudHeight = cloud->height;
int cloudWidth = cloud->width;
rgbFrame = Mat(cloudHeight, cloudWidth, CV_8UC3);
drawing = Mat(cloudHeight, cloudWidth, CV_8UC3, NULL);
grayFrame = Mat(cloudHeight, cloudWidth, CV_8UC1, NULL);
hsvFrame = Mat(cloudHeight, cloudWidth, CV_8UC3, NULL);
contourMask = Mat(cloudHeight, cloudWidth, CV_8UC1, NULL);
if (!cloud->empty())
{
for (int h = 0; h < rgbFrame.rows; h ++)
{
for (int w = 0; w < rgbFrame.cols; w++)
{
pcl::PointXYZRGBA point = cloud->at(w, cloudHeight-h-1);
Eigen::Vector3i rgb = point.getRGBVector3i();
rgbFrame.at<Vec3b>(h,w)[0] = rgb[2];
rgbFrame.at<Vec3b>(h,w)[1] = rgb[1];
rgbFrame.at<Vec3b>(h,w)[2] = rgb[0];
}
}
// do the filtering
int xPos = 0;
int yPos = 0;
mtx.lock();
xPos = mouse_x;
yPos = mouse_y;
mtx.unlock();
// color filtering based on what is chosen by users
cvtColor(rgbFrame, hsvFrame, CV_RGB2HSV);
Vec3b pixel = hsvFrame.at<Vec3b>(xPos,yPos);
int hueLow = pixel[0] < iHueDev ? pixel[0] : pixel[0] - iHueDev;
int hueHigh = pixel[0] > 255 - iHueDev ? pixel[0] : pixel[0] + iHueDev;
// inRange(hsvFrame, Scalar(hueLow, pixel[1]-20, pixel[2]-20), Scalar(hueHigh, pixel[1]+20, pixel[2]+20), grayFrame);
inRange(hsvFrame, Scalar(hueLow, iLowS, iLowV), Scalar(hueHigh, iHighS, iHighV), grayFrame);
// removes small objects from the foreground by morphological opening
erode(grayFrame, grayFrame, getStructuringElement(MORPH_ELLIPSE, Size(5,5)));
dilate(grayFrame, grayFrame, getStructuringElement(MORPH_ELLIPSE, Size(5,5)));
// morphological closing (removes small holes from the foreground)
dilate(grayFrame, grayFrame, getStructuringElement(MORPH_ELLIPSE, Size(5,5)));
erode(grayFrame, grayFrame, getStructuringElement(MORPH_ELLIPSE, Size(5,5)));
// gets contour from the grayFrame and keeps the largest contour
Mat cannyOutput;
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
int thresh = 100;
Canny(grayFrame, cannyOutput, thresh, thresh * 2, 3);
findContours(cannyOutput, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
int largestContourArea, largestContourIndex = 0;
int defaultContourArea = 1000; // 1000 seems to work find in most cases... cannot prove this
vector<vector<Point> > newContours;
for (int i = 0; i < contours.size(); i++)
{
double area = contourArea(contours[i], false);
if (area > defaultContourArea)
newContours.push_back(contours[i]);
}
// draws the largest contour:
drawing = Mat::zeros(cannyOutput.size(), CV_8UC3);
for (int i = 0; i < newContours.size(); i++)
drawContours(drawing, newContours, i, Scalar(255, 255, 255), CV_FILLED, 8, hierarchy, 0, Point());
// gets the filter by setting everything within the contour to be 1.
inRange(drawing, Scalar(1, 1, 1), Scalar(255, 255, 255), contourMask);
// filters the point cloud based on contourMask
// again go through the point cloud and filter out unnecessary points
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr resultCloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointXYZRGBA newPoint;
for (int h = 0; h < contourMask.rows; h ++)
{
for (int w = 0; w < contourMask.cols; w++)
{
if (contourMask.at<uchar>(h,w) > 0)
{
newPoint = cloud->at(w,h);
resultCloud->push_back(newPoint);
}
}
}
if (xPos == 0 && yPos == 0)
viewer.showCloud(cloud);
else
viewer.showCloud(resultCloud);
imshow("tracker", rgbFrame);
imshow("filtered result", contourMask);
char key = waitKey(1);
if (key == 27)
{
interface->stop();
return;
}
}
else
cout << "Warning: Point Cloud is empty" << endl;
}
else
cout << "Warning: Point Cloud is not organized" << endl;
}
}
inline void
hit_same_point( const image_geometry::PinholeCameraModel &camera_model,
const pcl::PointCloud<PointT> &in,
pcl::PointCloud<PointT> &out,
int rows,
int cols,
float z_threshold = 0.3)
{
std::vector<std::vector <std::vector<PointT> > > hit( cols, std::vector< std::vector<PointT> >(rows, std::vector<PointT>()));
// Project points into image space
for(unsigned int i=0; i < in.points.size(); ++i){
const PointT* pt = &in.points.at(i);
if( pt->z > 1) { // min distance from camera 1m
cv::Point2i point_image = camera_model.project3dToPixel(cv::Point3d(pt->x, pt->y, pt->z));
if( between<int>(0, point_image.x, cols )
&& between<int>( 0, point_image.y, rows )
)
{
// Sort all points into image
{
hit[point_image.x][point_image.y].push_back(in.points.at(i));
}
}
}
}
assert(out.empty());
for(int x = 0; x < hit.size(); ++x ){
for(int y = 0; y < hit[0].size(); ++y){
if(hit[x][y].size()>1){
PointT min_z = hit[x][y][0];
float max_z = min_z.z;
for(int p = 1; p < hit[x][y].size(); ++p){
// find min and max z
max_z = MAX(max_z, hit[x][y][p].z);
#ifdef DEBUG
std::cout << hit[x][y].size() << "\t";
#endif
if(hit[x][y][p].z < min_z.z)
{
min_z = hit[x][y][p];
}
}
#ifdef DEBUG
std::cout << min_z.z << "\t" << max_z << "\t";
#endif
if(max_z - min_z.z > z_threshold){
#ifdef DEBUG
std::cout << min_z << std::endl;
#endif
out.push_back(min_z);
}
}
}
}
#ifdef DEBUG
std::cout << "hit_same_point in: "<< in.size() << "\t out: " << out.size() << "\n";
#endif
}
template <typename PointT> int
pcl::PCDWriter::writeBinary (const std::string &file_name,
const pcl::PointCloud<PointT> &cloud)
{
if (cloud.empty ())
{
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data!");
return (-1);
}
int data_idx = 0;
std::ostringstream oss;
oss << generateHeader<PointT> (cloud) << "DATA binary\n";
oss.flush ();
data_idx = static_cast<int> (oss.tellp ());
#if _WIN32
HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
if(h_native_file == INVALID_HANDLE_VALUE)
{
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!");
return (-1);
}
#else
int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, static_cast<mode_t> (0600));
if (fd < 0)
{
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!");
return (-1);
}
#endif
std::vector<sensor_msgs::PointField> fields;
std::vector<int> fields_sizes;
size_t fsize = 0;
size_t data_size = 0;
size_t nri = 0;
pcl::getFields (cloud, fields);
// Compute the total size of the fields
for (size_t i = 0; i < fields.size (); ++i)
{
if (fields[i].name == "_")
continue;
int fs = fields[i].count * getFieldSize (fields[i].datatype);
fsize += fs;
fields_sizes.push_back (fs);
fields[nri++] = fields[i];
}
fields.resize (nri);
data_size = cloud.points.size () * fsize;
// Prepare the map
#if _WIN32
HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL);
char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
CloseHandle (fm);
#else
// Stretch the file size to the size of the data
int result = static_cast<int> (pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET));
if (result < 0)
{
pcl_close (fd);
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during lseek ()!");
return (-1);
}
// Write a bogus entry so that the new file size comes in effect
result = static_cast<int> (::write (fd, "", 1));
if (result != 1)
{
pcl_close (fd);
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during write ()!");
return (-1);
}
char *map = static_cast<char*> (mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
{
pcl_close (fd);
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!");
return (-1);
}
#endif
// Copy the header
memcpy (&map[0], oss.str ().c_str (), data_idx);
// Copy the data
char *out = &map[0] + data_idx;
for (size_t i = 0; i < cloud.points.size (); ++i)
{
int nrj = 0;
for (size_t j = 0; j < fields.size (); ++j)
{
memcpy (out, reinterpret_cast<const char*> (&cloud.points[i]) + fields[j].offset, fields_sizes[nrj]);
out += fields_sizes[nrj++];
}
}
// If the user set the synchronization flag on, call msync
#if !_WIN32
if (map_synchronization_)
msync (map, data_idx + data_size, MS_SYNC);
#endif
// Unmap the pages of memory
#if _WIN32
UnmapViewOfFile (map);
#else
if (munmap (map, (data_idx + data_size)) == -1)
{
pcl_close (fd);
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
return (-1);
}
#endif
// Close file
#if _WIN32
CloseHandle (h_native_file);
#else
pcl_close (fd);
#endif
return (0);
}
template<typename PointT> int
pcl::PCDWriter::appendBinary(const std::string &file_name,
const pcl::PointCloud<PointT> &cloud)
{
if(cloud.empty())
{
throw pcl::IOException("[pcl::PCDWriter::appendBinary] Input point cloud has no data!");
return -1;
}
if(!boost::filesystem::exists(file_name))
return writeBinary(file_name, cloud);
std::ifstream file_istream;
file_istream.open(file_name.c_str(), std::ifstream::binary);
if(!file_istream.good())
{
throw pcl::IOException("[pcl::PCDWriter::appendBinary] Error opening file for reading");
return -1;
}
file_istream.seekg(0, std::ios_base::end);
size_t file_size = file_istream.tellg();
file_istream.close();
pcl::PCLPointCloud2 tmp_cloud;
PCDReader reader;
if(reader.readHeader(file_name, tmp_cloud) != 0)
{
throw pcl::IOException("[pcl::PCDWriter::appendBinary] Failed reading header");
return -1;
}
if(tmp_cloud.height != 1 || cloud.height != 1)
{
throw pcl::IOException("[pcl::PCDWriter::appendBinary] can't use appendBinary with a point cloud that "
"has height different than 1!");
return -1;
}
tmp_cloud.width += cloud.width;
std::ostringstream oss;
pcl::PointCloud<PointT> tmp_cloud2;
// copy the header values:
tmp_cloud2.header = tmp_cloud.header;
tmp_cloud2.width = tmp_cloud.width;
tmp_cloud2.height = tmp_cloud.height;
tmp_cloud2.is_dense = tmp_cloud.is_dense;
oss << PCDWriter::generateHeader(tmp_cloud2, tmp_cloud2.width) << "DATA binary\n";
size_t data_idx = oss.tellp();
#if _WIN32
HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);
if (h_native_file == INVALID_HANDLE_VALUE)
{
throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during CreateFile!");
return (-1);
}
#else
int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_APPEND, static_cast<mode_t> (0600));
if (fd < 0)
{
throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during open!");
return (-1);
}
#endif
// Mandatory lock file
boost::interprocess::file_lock file_lock;
setLockingPermissions (file_name, file_lock);
std::vector<pcl::PCLPointField> fields;
std::vector<int> fields_sizes;
size_t fsize = 0;
size_t data_size = 0;
size_t nri = 0;
pcl::getFields (cloud, fields);
// Compute the total size of the fields
for (size_t i = 0; i < fields.size (); ++i)
{
if (fields[i].name == "_")
continue;
int fs = fields[i].count * getFieldSize (fields[i].datatype);
fsize += fs;
fields_sizes.push_back (fs);
fields[nri++] = fields[i];
}
fields.resize (nri);
data_size = cloud.points.size () * fsize;
data_idx += (tmp_cloud.width - cloud.width) * fsize;
if (data_idx != file_size)
{
const char *msg = "[pcl::PCDWriter::appendBinary] The expected data size and the current data size are different!";
PCL_WARN(msg);
throw pcl::IOException (msg);
return -1;
}
// Prepare the map
#if _WIN32
HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL);
char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
CloseHandle (fm);
#else
// Stretch the file size to the size of the data
off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);
if (result < 0)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
PCL_ERROR ("[pcl::PCDWriter::appendBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno));
throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during lseek ()!");
return (-1);
}
// Write a bogus entry so that the new file size comes in effect
result = static_cast<int> (::write (fd, "", 1));
if (result != 1)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during write ()!");
return (-1);
}
char *map = static_cast<char*> (mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during mmap ()!");
return (-1);
}
#endif
char* out = &map[0] + data_idx;
// Copy the data
for (size_t i = 0; i < cloud.points.size (); ++i)
{
int nrj = 0;
for (size_t j = 0; j < fields.size (); ++j)
{
memcpy (out, reinterpret_cast<const char*> (&cloud.points[i]) + fields[j].offset, fields_sizes[nrj]);
out += fields_sizes[nrj++];
}
}
// write the new header:
std::string header(oss.str());
memcpy(map, header.c_str(), header.size());
// If the user set the synchronization flag on, call msync
#if !_WIN32
if (map_synchronization_)
msync (map, data_idx + data_size, MS_SYNC);
#endif
// Unmap the pages of memory
#if _WIN32
UnmapViewOfFile (map);
#else
if (munmap (map, (data_idx + data_size)) == -1)
{
pcl_close (fd);
resetLockingPermissions (file_name, file_lock);
throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
return (-1);
}
#endif
// Close file
#if _WIN32
CloseHandle (h_native_file);
#else
pcl_close (fd);
#endif
resetLockingPermissions (file_name, file_lock);
return 0;
}
