| 7 | namespace util { |
| 8 | |
| 9 | void Plot::plotHandGeometry(const candidate::Hand &hand, |
| 10 | const PointCloudRGBA::Ptr &cloud, |
| 11 | const candidate::HandGeometry &hand_geom, |
| 12 | const descriptor::ImageGeometry &image_geom) { |
| 13 | Eigen::Vector3d vol_rgb(0.0, 0.8, 0.0); |
| 14 | Eigen::Vector3d hand_rgb(0.0, 0.5, 0.5); |
| 15 | PCLVisualizer viewer = createViewer("Hand Geometry"); |
| 16 | plotHand3D(viewer, hand, hand_geom, 0, hand_rgb); |
| 17 | Eigen::Vector3d vol_pos = |
| 18 | hand.getPosition() + 0.5 * image_geom.depth_ * hand.getApproach(); |
| 19 | Eigen::Quaterniond vol_quat(hand.getFrame()); |
| 20 | plotCube(viewer, vol_pos, vol_quat, image_geom.depth_, |
| 21 | image_geom.outer_diameter_, 2.0 * image_geom.height_, "volume", |
| 22 | vol_rgb); |
| 23 | |
| 24 | Eigen::Vector3d dimensions(hand_geom.depth_, hand_geom.outer_diameter_, |
| 25 | 2.0 * hand_geom.height_); |
| 26 | Eigen::Matrix3d colors; |
| 27 | colors << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; |
| 28 | Eigen::Vector3d center = hand.getPosition() - |
| 29 | hand_geom.height_ * hand.getAxis() - |
| 30 | 0.5 * hand_geom.outer_diameter_ * hand.getBinormal(); |
| 31 | std::vector<std::string> labels; |
| 32 | labels.push_back("hand_depth"); |
| 33 | labels.push_back("hand_outer_diameter"); |
| 34 | labels.push_back("hand_height * 2"); |
| 35 | addDimensions(center, hand.getOrientation(), dimensions, colors, labels, |
| 36 | viewer); |
| 37 | |
| 38 | Eigen::Vector3d p = center + 2.0 * hand_geom.height_ * hand.getAxis(); |
| 39 | Eigen::Vector3d q = p + hand_geom.finger_width_ * hand.getBinormal(); |
| 40 | addDoubleArrow(p, q, "finger_width", Eigen::Vector3d(0.0, 1.0, 1.0), viewer); |
| 41 | |
| 42 | dimensions << image_geom.depth_, -image_geom.outer_diameter_, |
| 43 | 2.0 * image_geom.height_; |
| 44 | colors *= 0.6; |
| 45 | center = hand.getPosition() - image_geom.height_ * hand.getAxis() + |
| 46 | 0.5 * image_geom.outer_diameter_ * hand.getBinormal(); |
| 47 | labels.resize(0); |
| 48 | labels.push_back("volume_depth"); |
| 49 | labels.push_back("volume_width"); |
| 50 | labels.push_back("volume_height * 2"); |
| 51 | addDimensions(center, hand.getOrientation(), dimensions, colors, labels, |
| 52 | viewer); |
| 53 | |
| 54 | pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgb( |
| 55 | cloud); |
| 56 | viewer->addPointCloud<pcl::PointXYZRGBA>(cloud, rgb, "cloud"); |
| 57 | viewer->setPointCloudRenderingProperties( |
| 58 | pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); |
| 59 | |
| 60 | runViewer(viewer); |
| 61 | } |
| 62 | |
| 63 | void Plot::addDimensions(const Eigen::Vector3d ¢er, |
| 64 | const Eigen::Matrix3d &rot, |