count = 0
points = np.zeros((100 * 100, 3), dtype=np.float32)
# float NG
for x in range(-50, 50, 1):
for y in range(-50, 50, 1):
points[count][0] = x * 0.01
points[count][1] = y * 0.01
points[count][2] = 2.0 - y * 0.01
count = count + 1
point_cloud = pcl.PointCloud()
point_cloud.from_array(points)
far_ranges = pcl.PointCloud_PointWithViewpoint()
##
# Create RangeImage from the PointCloud
noise_level = 0.0
min_range = 0.0
border_size = 1
# boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);
# pcl::RangeImage& range_image = *range_image_ptr;
# range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
# scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
# range_image.integrateFarRanges (far_ranges);
# if (setUnseenToMaxRange)
# range_image.setUnseenToMaxRange ();
def main():
# -----Main-----
# // --------------
# int main (int argc, char** argv)
# // -----Parse Command Line Arguments-----
# if (pcl::console::find_argument (argc, argv, "-h") >= 0)
# {
# printUsage (argv[0]);
# return 0;
# }
# if (pcl::console::find_argument (argc, argv, "-m") >= 0)
# {
# setUnseenToMaxRange = true;
# cout << "Setting unseen values in range image to maximum range readings.\n";
# }
# int tmp_coordinate_frame;
# if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
# {
# coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
# cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
# }
# if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
# cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
parser = argparse.ArgumentParser(
description='StrawPCL example: range image border extraction')
parser.add_argument(
'--UnseenToMaxRange',
'-m',
default=True,
type=bool,
help='Setting unseen values in range image to maximum range readings')
parser.add_argument('--CoordinateFrame',
'-c',
default=-1,
type=int,
help='Using coordinate frame = ')
parser.add_argument('--AngularResolution',
'-r',
default=0,
type=int,
help='Setting angular resolution to = ')
parser.add_argument(
'--Help',
help='Usage: narf_keypoint_extraction.py [options] <scene.pcd>\n\n'
'Options:\n'
'-------------------------------------------\n'
'-r <float> angular resolution in degrees (default = angular_resolution)\n'
'-c <int> coordinate frame (default = coordinate_frame)\n'
'-m Treat all unseen points as max range\n'
'-h this help\n\n\n;')
args = parser.parse_args()
setUnseenToMaxRange = args.UnseenToMaxRange
# coordinate_frame = pcl.RangeImage.CoordinateFrame (args.CoordinateFrame)
# angular_resolution = pcl.deg2rad (args.AngularResolution)
# // -----Read pcd file or create example point cloud if not given-----
# pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
# pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
point_cloud = pcl.PointCloud()
# pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
# Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
# scene_sensor_pose = (Eigen::Affine3f::Identity ())
# std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
pcd_filename_indices = [0, 0, 0]
# pcd_filename_indices = [0, 0, 0]
##
# if (!pcd_filename_indices.empty ())
# {
# std::string filename = argv[pcd_filename_indices[0]];
# if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
# {
# cout << "Was not able to open file \""<<filename<<"\".\n";
# printUsage (argv[0]);
# return 0;
# }
# scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
# point_cloud.sensor_origin_[1],
# point_cloud.sensor_origin_[2])) *
# Eigen::Affine3f (point_cloud.sensor_orientation_);
# std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";
# if (pcl::io::loadPCDFile(far_ranges_filename.c_str(), far_ranges) == -1)
# std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
# }
# else
# {
# cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
# for (float x=-0.5f; x<=0.5f; x+=0.01f)
# {
# for (float y=-0.5f; y<=0.5f; y+=0.01f)
# {
# PointType point; point.x = x; point.y = y; point.z = 2.0f - y;
# point_cloud.points.push_back (point);
# }
# }
# point_cloud.width = (int) point_cloud.points.size (); point_cloud.height = 1;
# }
if len(pcd_filename_indices) != 0:
# point_cloud = pcl.load(argv[0])
point_cloud = pcl.load('test_pcd.pcd')
far_ranges_filename = 'test_pcd.pcd'
far_ranges = pcl.load_PointWithViewpoint(far_ranges_filename)
else:
setUnseenToMaxRange = True
print('No *.pcd file given = Genarating example point cloud.\n')
count = 0
points = np.zeros((100 * 100, 3), dtype=np.float32)
# float NG
for x in range(-50, 50, 1):
for y in range(-50, 50, 1):
points[count][0] = x * 0.01
points[count][1] = y * 0.01
points[count][2] = 2.0 - y * 0.01
count = count + 1
point_cloud = pcl.PointCloud()
point_cloud.from_array(points)
far_ranges = pcl.PointCloud_PointWithViewpoint()
##
# Create RangeImage from the PointCloud
noise_level = 0.0
min_range = 0.0
border_size = 1
# boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);
# pcl::RangeImage& range_image = *range_image_ptr;
# range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
# scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
# range_image.integrateFarRanges (far_ranges);
# if (setUnseenToMaxRange)
# range_image.setUnseenToMaxRange ();
##
range_image = point_cloud.make_RangeImage()
range_image.CreateFromPointCloud(point_cloud, angular_resolution,
pcl.deg2rad(360.0), pcl.deg2rad(180.0),
coordinate_frame, noise_level, min_range,
border_size)
print('range_image::integrateFarRanges.\n')
if setUnseenToMaxRange == True:
range_image.SetUnseenToMaxRange()
# -----Open 3D viewer and add point cloud-----
# pcl::visualization::PCLVisualizer viewer ("3D Viewer");
# viewer.setBackgroundColor (1, 1, 1);
# viewer.addCoordinateSystem (1.0f, "global");
# pcl::visualization::PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 0, 0, 0);
# viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
# // PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 150, 150, 150);
# // viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
# // viewer.setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 2, "range image");
##
# viewer = pcl.pcl_visualization.PCLVisualizering('3D Viewer')
viewer = pcl.pcl_visualization.PCLVisualizering()
viewer.SetBackgroundColor(1.0, 1.0, 1.0)
range_image_color_handler = pcl.pcl_visualization.PointCloudColorHandleringCustom(
point_cloud, 0, 0, 0)
# viewer.AddPointCloud (range_image, range_image_color_handler, 'range image')
viewer.AddPointCloud(point_cloud, 'range image')
# viewer.AddPointCloud_ColorHandler
# viewer.SetPointCloudRenderingProperties (pcl.pcl_visualization.PCLVISUALIZER_POINT_SIZE, 1, propName = 'range image')
# NG - ([setPointCloudRenderingProperties] Could not find any PointCloud datasets with id <cloud>!)
# viewer.SetPointCloudRenderingProperties (pcl.pcl_visualization.PCLVISUALIZER_POINT_SIZE, 1)
##
# Extract borders
# pcl::RangeImageBorderExtractor border_extractor (&range_image);
# pcl::PointCloud<pcl::BorderDescription> border_descriptions;
# border_extractor.compute (border_descriptions);
##
# // ----------------------------------
# // -----Show points in 3D viewer-----
# // ----------------------------------
# pcl::PointCloud<pcl::PointWithRange>::Ptr border_points_ptr(new pcl::PointCloud<pcl::PointWithRange>),
# veil_points_ptr(new pcl::PointCloud<pcl::PointWithRange>),
# shadow_points_ptr(new pcl::PointCloud<pcl::PointWithRange>);
# pcl::PointCloud<pcl::PointWithRange>& border_points = *border_points_ptr,
# & veil_points = * veil_points_ptr,
# & shadow_points = *shadow_points_ptr;
# for (int y=0; y< (int)range_image.height; ++y)
# {
# for (int x=0; x< (int)range_image.width; ++x)
# {
# if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__OBSTACLE_BORDER])
# border_points.points.push_back (range_image.points[y*range_image.width + x]);
# if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__VEIL_POINT])
# veil_points.points.push_back (range_image.points[y*range_image.width + x]);
# if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__SHADOW_BORDER])
# shadow_points.points.push_back (range_image.points[y*range_image.width + x]);
# }
# }
# pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> border_points_color_handler (border_points_ptr, 0, 255, 0);
# viewer.addPointCloud<pcl::PointWithRange> (border_points_ptr, border_points_color_handler, "border points");
# viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "border points");
# pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> veil_points_color_handler (veil_points_ptr, 255, 0, 0);
# viewer.addPointCloud<pcl::PointWithRange> (veil_points_ptr, veil_points_color_handler, "veil points");
# viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "veil points");
# pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> shadow_points_color_handler (shadow_points_ptr, 0, 255, 255);
# viewer.addPointCloud<pcl::PointWithRange> (shadow_points_ptr, shadow_points_color_handler, "shadow points");
# viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "shadow points");
#
##
# // -----Show points on range image-----
# pcl::visualization::RangeImageVisualizer* range_image_borders_widget = NULL;
# range_image_borders_widget =
# pcl::visualization::RangeImageVisualizer::getRangeImageBordersWidget (range_image, -std::numeric_limits<float>::infinity (), std::numeric_limits<float>::infinity (), false,
# border_descriptions, "Range image with borders");
#
##
# // -----Main loop-----
# while (!viewer.wasStopped ())
# {
# range_image_borders_widget->spinOnce ();
# viewer.spinOnce ();
# pcl_sleep(0.01);
# }
v = True
while v:
v = not (viewer.WasStopped())
viewer.SpinOnce()
def main():
# -----Main-----
# -----Parse Command Line Arguments-----
parser = argparse.ArgumentParser(
description='PointCloudLibrary example: narf keyPoint extraction')
parser.add_argument(
'--UnseenToMaxRange',
'-m',
default=True,
type=bool,
help='Setting unseen values in range image to maximum range readings')
parser.add_argument('--CoordinateFrame',
'-c',
default=-1,
type=int,
help='Using coordinate frame = ')
parser.add_argument('--SupportSize',
'-s',
default=0,
type=int,
help='Setting support size to = ')
parser.add_argument('--AngularResolution',
'-r',
default=0,
type=int,
help='Setting angular resolution to = ')
parser.add_argument(
'--Help',
help='Usage: narf_keypoint_extraction.py [options] <scene.pcd>\n\n'
'Options:\n'
'-------------------------------------------\n'
'-r <float> angular resolution in degrees (default = angular_resolution)\n'
'-c <int> coordinate frame (default = coordinate_frame)\n'
'-m Treat all unseen points as maximum range readings\n'
'-s <float> support size for the interest points (diameter of the used sphere - default = support_size)\n'
'-h this help\n\n\n')
args = parser.parse_args()
# args setting
setUnseenToMaxRange = args.UnseenToMaxRange
# coordinate_frame = pcl.RangeImage.CoordinateFrame (args.CoordinateFrame)
# angular_resolution = pcl.deg2rad (args.AngularResolution)
# -----Read pcd file or create example point cloud if not given-----
# pcl::PointCloudPointTypePtr point_cloud_ptr (new pcl::PointCloud::PointType);
# pcl::PointCloudPointType& point_cloud = point_cloud_ptr
# pcl::PointCloud<pcl::PointWithViewpoint> far_ranges
##
# point_cloud = pcl.PointCloud()
# Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ())
# scene_sensor_pose = (eigen3.Affine3f.Identity ())
# vector[int] pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, pcd)
# pcd_filename_indices = './examples/official/IO/test_pcd.pcd'
# pcd_filename_indices = [0, 0, 0]
# if pcd_filename_indices.empty() == False
pcd_filename_indices = ''
if len(pcd_filename_indices) != 0:
# # string filename = argv[pcd_filename_indices[0]]
# filename = argv[pcd_filename_indices[0]]
# point_cloud = pcl.load(argv[0])
point_cloud = pcl.load('./examples/official/IO/test_pcd.pcd')
# scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
# point_cloud.sensor_origin_[1],
# point_cloud.sensor_origin_[2])) *
# Eigen::Affine3f (point_cloud.sensor_orientation_);
# Python
# origin = point_cloud.sensor_origin
# sensor_orientation = eigen3.Affine3f(origin[0], origin[1], origin[2]) * eigen3.Affine3f(point_cloud.sensor_orientation)
# std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+_far_ranges.pcd;
# if (pcl::io::loadPCDFile (far_ranges_filename.c_str (), far_ranges) == -1)
# stdcout Far ranges file far_ranges_filename does not exists.n;
far_ranges_filename = os.path.splitext(
pcd_filename_indices) + '_far_ranges.pcd'
far_ranges = pcl.load_PointWithViewpoint(far_ranges_filename)
# Error
# print('Far ranges file ' + far_ranges_filename + 'does not exists.\n')
else:
setUnseenToMaxRange = True
print('No *.pcd file given = Genarating example point cloud.\n')
# for (float x = -0.5f; x = 0.5f; x += 0.01f)
# for (float y = -0.5f; y = 0.5f; y += 0.01f)
# points = np.zeros((1, 3), dtype=np.float32)
# points[0][0] = x
# points[0][1] = y
# points[0][2] = 2.0f - y
# end
# end
count = 0
points = np.zeros((100 * 100, 3), dtype=np.float32)
# float NG
# TypeError: range() integer end argument expected, got float.
# for x in range(-0.5, 0.5, 0.01):
# for y in range(-0.5, 0.5, 0.01):
for x in range(-50, 50, 1):
for y in range(-50, 50, 1):
points[count][0] = x * 0.01
points[count][1] = y * 0.01
points[count][2] = 2.0 - y * 0.01
count = count + 1
# point_cloud.points.push_back (point);
# point_cloud.width = (int) point_cloud.points.size ()
# point_cloud.height = 1;
point_cloud = pcl.PointCloud()
point_cloud.from_array(points)
far_ranges = pcl.PointCloud_PointWithViewpoint()
# Create RangeImage from the PointCloud
noise_level = 0.0
min_range = 0.0
border_size = 1
# boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);
# pcl::RangeImage& range_image = *range_image_ptr;
range_image = point_cloud.make_RangeImage()
print('range_image::createFromPointCloud.\n')
print('point_cloud(size ) = ' + str(point_cloud.size))
print('point_cloud(width ) = ' + str(point_cloud.width))
print('point_cloud(height) = ' + str(point_cloud.height))
# range_image.createFromPointCloud (
# point_cloud, angular_resolution, pcl.deg2rad (360.0f), pcl.deg2rad (180.0f),
# scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
range_image.CreateFromPointCloud(point_cloud, angular_resolution,
pcl.deg2rad(360.0), pcl.deg2rad(180.0),
coordinate_frame, noise_level, min_range,
border_size)
# NG
# print ('range_image::integrateFarRanges.\n')
# range_image.IntegrateFarRanges (far_ranges)
# if (setUnseenToMaxRange)
# range_image.setUnseenToMaxRange ();
print('range_image::setUnseenToMaxRange.\n')
if setUnseenToMaxRange == True:
range_image.SetUnseenToMaxRange()
# current(0.3.0) Windows Only Test
isVisual = False
try:
import pcl.pcl_visualization
isVisual = True
except:
print("Cannot import pcl.pcl_visualization")
import pcl.pcl_visualization
if isVisual == True:
# Open 3D viewer and add point cloud
# pcl::visualization::PCLVisualizer viewer ("3D Viewer")
# viewer.setBackgroundColor (1, 1, 1)
# pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
# viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
# viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
# viewer.initCameraParameters ();
##
# viewer = pcl.PCLVisualizering("3D Viewer")
viewer = pcl.pcl_visualization.PCLVisualizering('3D Viewer')
viewer.SetBackgroundColor(1, 1, 1)
# NG
# range_image_color_handler = pcl.PointCloudColorHandlerCustoms[cpp.PointWithRange] (range_image, 0, 0, 0)
# range_image_color_handler = pcl.PointCloudColorHandlerCustoms (range_image, 0, 0, 0)
# range_image_color_handler = pcl.pcl_visualization.PointCloudColorHandleringCustom (range_image, 0, 0, 0)
range_image_color_handler = pcl.pcl_visualization.PointCloudColorHandleringCustom(
point_cloud, 0, 0, 0)
viewer.AddPointCloud_ColorHandler(point_cloud,
range_image_color_handler,
b'range image')
# viewer.AddPointCloud (point_cloud, 'range image', 0)
# viewer.AddPointCloud (point_cloud)
time.sleep(1)
viewer.SetPointCloudRenderingProperties(
pcl.pcl_visualization.PCLVISUALIZER_POINT_SIZE, 1, b'range image')
time.sleep(1)
viewer.InitCameraParameters()
time.sleep(1)
# Show range image
# pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
# range_image_widget.showRangeImage (range_image);
range_image_widget = pcl.pcl_visualization.RangeImageVisualization()
range_image_widget.ShowRangeImage(range_image)
# Extract NARF keypoints
# pcl::RangeImageBorderExtractor range_image_border_extractor;
# pcl::NarfKeypoint narf_keypoint_detector (&range_image_border_extractor);
# narf_keypoint_detector.setRangeImage (&range_image);
# narf_keypoint_detector.getParameters ().support_size = support_size;
# narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;
# narf_keypoint_detector.getParameters ().distance_for_additional_points = 0.5;
# pcl::PointCloud<int> keypoint_indices;
# narf_keypoint_detector.compute (keypoint_indices);
# std::cout << "Found" << keypoint_indices.points.size () << "key points.\n";
range_image_border_extractor = pcl.RangeImageBorderExtractor()
narf_keypoint_detector = pcl.NarfKeypoint(range_image_border_extractor)
# narf_keypoint_detector.SetRangeImage (&range_image)
# pcl::PointCloud<int> keypoint_indices;
# narf_keypoint_detector.compute (keypoint_indices);
print("Found" + str(keypoint_indices.size) + "key points.\n")
# Show keypoints in range image widget
### Comment ###
# for (size_t i=0; ikeypoint_indices.points.size (); ++i)
# range_image_widget.markPoint (keypoint_indices.points[i] % range_image.width,
# keypoint_indices.points[i], range_image.width);
# for size_t i=0; ikeypoint_indices.points.size (); ++i:
# range_image_widget.markPoint (keypoint_indices.points[i] % range_image.width, keypoint_indices.points[i], range_image.width)
###
# Show keypoints in 3D viewer
# pcl::PointCloud<pcl::PointXYZPtr> keypoints_ptr (new pclPointCloudpclPointXYZ);
# pcl::PointCloud<pcl::PointXYZ> &keypoints = keypoints_ptr;
# keypoints.points.resize (keypoint_indices.points.size ());
# for (size_t i=0; ikeypoint_indices.points.size (); ++i)
# keypoints.points[i].getVector3fMap () = range_image.points[keypoint_indices.points[i]].getVector3fMap ();
##
keypoints = pcl.KeyPoints()
keypoints.resize(keypoint_indices.size)
# for i in range(0, keypoint_indices.size, 1):
# keypoints.points[i].getVector3fMap () = range_image[keypoint_indices.points[i]].getVector3fMap ()
# for x in range(-50, 50, 1):
# for y in range(-50, 50, 1):
# pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (keypoints_ptr, 0, 255, 0);
# viewer.addPointCloud<pcl::PointXYZ> (keypoints_ptr, keypoints_color_handler, keypoints);
# viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, keypoints);
# keypoints_color_handler = pcl.PointCloudColorHandlerCustom (0, 255, 0)
# viewer.AddPointCloud<pcl::PointXYZ} (keypoints_ptr, keypoints_color_handler, keypoints)
# viewer.SetPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, keypoints);
# pcl
# keypoints_color_handler = pcl.visualization.PointCloudColorHandlerCustom[pcl.PointXYZ](keypoints_ptr, 0, 255, 0)
viewer.addPointCloud(keypoints_ptr, keypoints_color_handler, keypoints)
viewer.setPointCloudRenderingProperties(
pcl.pcl_visualization.PCL_VISUALIZER_POINT_SIZE, 7, keypoints)
keypoints_color_handler = pcl.PointCloudColorHandlerCustom(0, 255, 0)
viewer.AddPointCloud(keypoints_ptr, keypoints_color_handler, keypoints)
viewer.SetPointCloudRenderingProperties(
pcl.pcl_visualization.PCL_VISUALIZER_POINT_SIZE, 7, keypoints)
# Main loop
print("while")
v = True
while v:
v = not (viewer.WasStopped())
# process GUI events
range_image_widget.SpinOnce()
viewer.SpinOnce()
else:
pass
def main():
# -----Main-----
# // -----Parse Command Line Arguments-----
# if (pcl::console::find_argument (argc, argv, "-h") >= 0)
# {
# printUsage (argv[0]);
# return 0;
# }
# if (pcl::console::find_argument (argc, argv, "-l") >= 0)
# {
# live_update = true;
# std::cout << "Live update is on.\n";
# }
# if (pcl::console::parse (argc, argv, "-rx", angular_resolution_x) >= 0)
# std::cout << "Setting angular resolution in x-direction to "<<angular_resolution_x<<"deg.\n";
# if (pcl::console::parse (argc, argv, "-ry", angular_resolution_y) >= 0)
# std::cout << "Setting angular resolution in y-direction to "<<angular_resolution_y<<"deg.\n";
# int tmp_coordinate_frame;
# if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
# {
# coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
# std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
# }
# angular_resolution_x = pcl::deg2rad (angular_resolution_x);
# angular_resolution_y = pcl::deg2rad (angular_resolution_y);
parser = argparse.ArgumentParser(
description='StrawPCL example: How to visualize a range image')
parser.add_argument('--UnseenToMaxRange', '-m', default=True, type=bool,
help='Setting unseen values in range image to maximum range readings')
parser.add_argument('--CoordinateFrame', '-c', default=-1, type=int,
help='Using coordinate frame = ')
parser.add_argument('--AngularResolution', '-r', default=0, type=int,
help='Setting angular resolution to = ')
parser.add_argument('--Help',
help='Usage: narf_keypoint_extraction.py [options] <scene.pcd>\n\n'
'Options:\n'
'-------------------------------------------\n'
'-r <float> angular resolution in degrees (default = angular_resolution)\n'
'-c <int> coordinate frame (default = coordinate_frame)\n'
'-m Treat all unseen points as max range\n'
'-h this help\n\n\n;')
args = parser.parse_args()
# // -----Read pcd file or create example point cloud if not given-----
# pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
# pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
# Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
# std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
# if (!pcd_filename_indices.empty ())
# {
# std::string filename = argv[pcd_filename_indices[0]];
# if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
# {
# std::cout << "Was not able to open file \""<<filename<<"\".\n";
# printUsage (argv[0]);
# return 0;
# }
# scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
# point_cloud.sensor_origin_[1],
# point_cloud.sensor_origin_[2])) *
# Eigen::Affine3f (point_cloud.sensor_orientation_);
# }
# else
# {
# std::cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
# for (float x=-0.5f; x<=0.5f; x+=0.01f)
# {
# for (float y=-0.5f; y<=0.5f; y+=0.01f)
# {
# PointType point; point.x = x; point.y = y; point.z = 2.0f - y;
# point_cloud.points.push_back (point);
# }
# }
# point_cloud.width = (int) point_cloud.points.size (); point_cloud.height = 1;
# }
pcd_filename_indices = ""
if len(pcd_filename_indices) != 0:
# point_cloud = pcl.load(argv[0])
point_cloud = pcl.load('test_pcd.pcd')
far_ranges_filename = 'test_pcd.pcd'
far_ranges = pcl.load_PointWithViewpoint(far_ranges_filename)
print("aqui")
else:
setUnseenToMaxRange = True
print('No *.pcd file given = Genarating example point cloud.\n')
count = 0
points = np.zeros((100 * 100, 3), dtype=np.float32)
# float NG
for x in range(-50, 50, 1):
for y in range(-50, 50, 1):
points[count][0] = x * 0.01
points[count][1] = y * 0.01
points[count][2] = 2.0 - y * 0.01
count = count + 1
point_cloud = pcl.PointCloud()
point_cloud.from_array(points)
far_ranges = pcl.PointCloud_PointWithViewpoint()
# // -----------------------------------------------
# // -----Create RangeImage from the PointCloud-----
# // -----------------------------------------------
# float noise_level = 0.0;
# float min_range = 0.0f;
# int border_size = 1;
# boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
# pcl::RangeImage& range_image = *range_image_ptr;
# range_image.createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y,
# pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
# scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
noise_level = 0.0
min_range = 0.0
border_size = 1
range_image = point_cloud.make_RangeImage()
range_image.CreateFromPointCloud(point_cloud,
angular_resolution_x, pcl.deg2rad(
360.0), pcl.deg2rad(180.0),
coordinate_frame, noise_level, min_range, border_size)
print('AQUI')
print('range_image::integrateFarRanges.\n')
# // --------------------------------------------
# // -----Open 3D viewer and add point cloud-----
# // --------------------------------------------
# pcl::visualization::PCLVisualizer viewer ("3D Viewer");
# viewer.setBackgroundColor (1, 1, 1);
# pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
# viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
# viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
# //viewer.addCoordinateSystem (1.0f, "global");
# //PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
# //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
# viewer.initCameraParameters ();
# setViewerPose(viewer, range_image.getTransformationToWorldSystem ());
viewer = pcl.pcl_visualization.PCLVisualizering()
viewer.SetBackgroundColor(1.0, 1.0, 1.0)
range_image_color_handler = pcl.pcl_visualization.PointCloudColorHandleringCustom(
point_cloud, 0, 0, 0)
cloudname = str('cloud')
viewer.AddPointCloud(range_image, range_image_color_handler, cloudname)
viewer.SetPointCloudRenderingProperties(
pcl.pcl_visualization.PCLVISUALIZER_POINT_SIZE, 1, cloudname)
# // --------------------------
# // -----Show range image-----
# // --------------------------
# pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
# range_image_widget.showRangeImage (range_image);
range_image_widget = pcl.pcl_visualization.RangeImageVisualization()
range_image_widget.ShowRangeImage(range_image)