void pcl::people::PersonCluster<PointT>::drawTBoundingBox (pcl::visualization::PCLVisualizer& viewer, int person_number) { // draw theoretical person bounding box in the PCL viewer: pcl::ModelCoefficients coeffs; // translation coeffs.values.push_back (tcenter_[0]); coeffs.values.push_back (tcenter_[1]); coeffs.values.push_back (tcenter_[2]); // rotation coeffs.values.push_back (0.0); coeffs.values.push_back (0.0); coeffs.values.push_back (0.0); coeffs.values.push_back (1.0); // size if (vertical_) { coeffs.values.push_back (height_); coeffs.values.push_back (0.5); coeffs.values.push_back (0.5); } else { coeffs.values.push_back (0.5); coeffs.values.push_back (height_); coeffs.values.push_back (0.5); } std::stringstream bbox_name; bbox_name << "bbox_person_" << person_number; viewer.removeShape (bbox_name.str()); viewer.addCube (coeffs, bbox_name.str()); viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, bbox_name.str()); viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 2, bbox_name.str()); // std::stringstream confid; // confid << person_confidence_; // PointT position; // position.x = tcenter_[0]- 0.2; // position.y = ttop_[1]; // position.z = tcenter_[2]; // viewer.addText3D(confid.str().substr(0, 4), position, 0.1); }
int main (int argc, char const* argv[]) { if (argc != 2) { cout << "Usage : obb_test filename.pcd" << endl; return 1; } pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud) == -1) { PCL_ERROR ("Couldn't read file test_pcd.pcd \n"); return 1; } cloud_viewer.addPointCloud (cloud, "single_cloud"); OrientedBoundingBox obb; Eigen::Quaternionf q; Eigen::Vector3f t, dims; obb.compute_obb_pca (cloud, q, t, dims); cloud_viewer.addCube(t, q, dims.x(), dims.y(), dims.z()); cout << dims.x() << " " << dims.y() << " " << dims.z() << endl; while (!cloud_viewer.wasStopped()) { cloud_viewer.spinOnce(1); } return 0; }
void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun) { const std::string &cloudname = this->name; if(firstRun) { visualizer.addPointCloud(cloud, cloudname); visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname); } else { visualizer.updatePointCloud(cloud, cloudname); visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname); visualizer.removeAllShapes(); } visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0.2, 0, 0), 1, 0, 0, "X"); visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0, 0.2, 0), 0, 1, 0, "Y"); visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0, 0, 0.2), 0, 0, 1, "Z"); tf::Vector3 origin = worldToCam * tf::Vector3(0, 0, 0); tf::Vector3 lineX = worldToCam * tf::Vector3(0.2, 0, 0); tf::Vector3 lineY = worldToCam * tf::Vector3(0, 0.2, 0); tf::Vector3 lineZ = worldToCam * tf::Vector3(0, 0, 0.2); pcl::PointXYZ pclOrigin(origin.x(), origin.y(), origin.z()); pcl::PointXYZ pclLineX(lineX.x(), lineX.y(), lineX.z()); pcl::PointXYZ pclLineY(lineY.x(), lineY.y(), lineY.z()); pcl::PointXYZ pclLineZ(lineZ.x(), lineZ.y(), lineZ.z()); visualizer.addLine(pcl::PointXYZ(0, 0, 0), pclOrigin, 1, 1, 1, "line"); visualizer.addLine(pclOrigin, pclLineX, 1, 0, 0, "lineX"); visualizer.addLine(pclOrigin, pclLineY, 0, 1, 0, "lineY"); visualizer.addLine(pclOrigin, pclLineZ, 0, 0, 1, "lineZ"); for(int i = 0; i < regions.size(); ++i) { const Region ®ion = regions[i]; tf::Transform transform = worldToCam * region.transform; std::ostringstream oss; oss << "region_" << i; tf::Vector3 originB = transform * tf::Vector3(0, 0, 0); tf::Vector3 lineXB = transform * tf::Vector3(0.2, 0, 0); tf::Vector3 lineYB = transform * tf::Vector3(0, 0.2, 0); tf::Vector3 lineZB = transform * tf::Vector3(0, 0, 0.2); pcl::PointXYZ pclOriginB(originB.x(), originB.y(), originB.z()); pcl::PointXYZ pclLineXB(lineXB.x(), lineXB.y(), lineXB.z()); pcl::PointXYZ pclLineYB(lineYB.x(), lineYB.y(), lineYB.z()); pcl::PointXYZ pclLineZB(lineZB.x(), lineZB.y(), lineZB.z()); visualizer.addLine(pclOrigin, pclOriginB, 1, 1, 1, "line_" + oss.str()); visualizer.addLine(pclOriginB, pclLineXB, 1, 0, 0, "lineX_" + oss.str()); visualizer.addLine(pclOriginB, pclLineYB, 0, 1, 0, "lineY_" + oss.str()); visualizer.addLine(pclOriginB, pclLineZB, 0, 0, 1, "lineZ_" + oss.str()); Eigen::Vector3d translation; Eigen::Quaterniond rotation; tf::vectorTFToEigen(transform.getOrigin(), translation); tf::quaternionTFToEigen(transform.getRotation(), rotation); visualizer.addCube(translation.cast<float>(), rotation.cast<float>(), region.width, region.height, region.depth, oss.str()); } }