| 1750 | |
| 1751 | template <typename PointT> |
| 1752 | void MapViewer<PointT>::update_submap_node(cloudblock_Ptrs &submaps, boost::shared_ptr<pcl::visualization::PCLVisualizer> &viewer, |
| 1753 | int display_time_ms) |
| 1754 | { |
| 1755 | pcl::PointCloud<pcl::PointXYZRGB>::Ptr nodes(new pcl::PointCloud<pcl::PointXYZRGB>()); |
| 1756 | for (int i = 0; i < submaps.size(); i++) |
| 1757 | { |
| 1758 | float r, g, b; |
| 1759 | pcl::PointXYZRGB ptc1_rgb; |
| 1760 | ptc1_rgb.x = submaps[i]->pose_lo(0, 3); |
| 1761 | ptc1_rgb.y = submaps[i]->pose_lo(1, 3); |
| 1762 | ptc1_rgb.z = submaps[i]->pose_lo(2, 3); |
| 1763 | get_random_color(r, g, b, 255); |
| 1764 | ptc1_rgb.r = r; |
| 1765 | ptc1_rgb.g = g; |
| 1766 | ptc1_rgb.b = b; |
| 1767 | nodes->points.push_back(ptc1_rgb); |
| 1768 | |
| 1769 | pcl::PointXYZRGB ptc2_rgb; |
| 1770 | ptc2_rgb.x = submaps[i]->pose_gt(0, 3); |
| 1771 | ptc2_rgb.y = submaps[i]->pose_gt(1, 3); |
| 1772 | ptc2_rgb.z = submaps[i]->pose_gt(2, 3); |
| 1773 | ptc2_rgb.r = r; |
| 1774 | ptc2_rgb.g = g; |
| 1775 | ptc2_rgb.b = b; |
| 1776 | nodes->points.push_back(ptc2_rgb); |
| 1777 | } |
| 1778 | viewer->addPointCloud(nodes, "nodes"); |
| 1779 | viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 8.0, "nodes"); |
| 1780 | |
| 1781 | judge_pause(viewer, display_time_ms); |
| 1782 | } |
| 1783 | |
| 1784 | template <typename PointT> |
| 1785 | void MapViewer<PointT>::update_lo_pose(Matrix4ds &poses_lo, Matrix4ds &poses_gt, |