1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
|
# Moment of inertia and eccentricity based descriptors
# http://pointclouds.org/documentation/tutorials/moment_of_inertia.php#moment-of-inertia
import pcl
# int main (int argc, char** argv)
# if (argc != 2)
# return (0);
cloud = pcl.load('./examples/pcldata/tutorials/lamppost.pcd')
# 1.8
# pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
feature_extractor = cloud.make_MomentOfInertiaEstimation()
# feature_extractor.setInputCloud (cloud)
feature_extractor.compute ()
# std::vector <float> moment_of_inertia;
# std::vector <float> eccentricity;
# pcl::PointXYZ min_point_AABB;
# pcl::PointXYZ max_point_AABB;
# pcl::PointXYZ min_point_OBB;
# pcl::PointXYZ max_point_OBB;
# pcl::PointXYZ position_OBB;
# Eigen::Matrix3f rotational_matrix_OBB;
# float major_value, middle_value, minor_value;
# Eigen::Vector3f major_vector, middle_vector, minor_vector;
# Eigen::Vector3f mass_center;
#
# feature_extractor.getMomentOfInertia (moment_of_inertia);
# feature_extractor.getEccentricity (eccentricity);
# feature_extractor.getAABB (min_point_AABB, max_point_AABB);
# feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
# feature_extractor.getEigenValues (major_value, middle_value, minor_value);
# feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
# feature_extractor.getMassCenter (mass_center);
###
moment_of_inertia = feature_extractor.get_MomentOfInertia ()
eccentricity = feature_extractor.get_Eccentricity ()
[min_point_AABB, max_point_AABB] = feature_extractor.get_AABB ();
[min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB] = feature_extractor.get_OBB ()
[major_value, middle_value, minor_value] = feature_extractor.get_EigenValues ()
[major_vector, middle_vector, minor_vector] = feature_extractor.get_EigenVectors ()
mass_center = feature_extractor.get_MassCenter ()
# View
# boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
# viewer->setBackgroundColor (0, 0, 0);
# viewer->addCoordinateSystem (1.0);
# viewer->initCameraParameters ();
# viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
# viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");
###
viewer = pcl.visualization.PCLVisualizing()
viewer.SetBackgroundColor(0, 0, 0)
viewer->InitCameraParameters ()
# viewer->AddPointCloud (cloud, 'sample cloud', 0)
viewer->AddPointCloud (cloud)
viewer.AddCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");
# Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
# Eigen::Quaternionf quat (rotational_matrix_OBB);
# viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
viewer.AddCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");
# pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
# pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
# pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
# pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
# viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
# viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
# viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");
center = pcl.PointCloud(mass_center[0], mass_center[1], mass_center[2])
x_axis = pcl.PointCloud(major_vector [0] + mass_center [0], major_vector [1] + mass_center [1], major_vector [2] + mass_center [2])
y_axis = pcl.PointCloud(middle_vector [0] + mass_center [0], middle_vector [1] + mass_center [1], middle_vector [2] + mass_center [2])
z_axis = pcl.PointCloud(minor_vector [0] + mass_center [0], minor_vector [1] + mass_center [1], minor_vector [2] + mass_center [2])
viewer.AddLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
viewer.AddLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
viewer.AddLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");
# while(!viewer->wasStopped())
# {
# viewer->spinOnce (100);
# boost::this_thread::sleep (boost::posix_time::microseconds (100000));
# }
while True:
visual.WasStopped()
# visual.spinOnce (100);
# boost::this_thread::sleep (boost::posix_time::microseconds (100000));
end
|
