| 101 | } |
| 102 | |
| 103 | void CameraPoseVisualization::add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) { |
| 104 | visualization_msgs::msg::Marker marker; |
| 105 | |
| 106 | marker.ns = m_marker_ns; |
| 107 | marker.id = 0; |
| 108 | marker.type = visualization_msgs::msg::Marker::LINE_STRIP; |
| 109 | marker.action = visualization_msgs::msg::Marker::ADD; |
| 110 | marker.scale.x = m_line_width; |
| 111 | |
| 112 | marker.pose.position.x = 0.0; |
| 113 | marker.pose.position.y = 0.0; |
| 114 | marker.pose.position.z = 0.0; |
| 115 | marker.pose.orientation.w = 1.0; |
| 116 | marker.pose.orientation.x = 0.0; |
| 117 | marker.pose.orientation.y = 0.0; |
| 118 | marker.pose.orientation.z = 0.0; |
| 119 | |
| 120 | geometry_msgs::msg::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2; |
| 121 | |
| 122 | Eigen2Point(q * (m_scale * imlt) + p, pt_lt); |
| 123 | Eigen2Point(q * (m_scale * imlb) + p, pt_lb); |
| 124 | Eigen2Point(q * (m_scale * imrt) + p, pt_rt); |
| 125 | Eigen2Point(q * (m_scale * imrb) + p, pt_rb); |
| 126 | Eigen2Point(q * (m_scale * lt0) + p, pt_lt0); |
| 127 | Eigen2Point(q * (m_scale * lt1) + p, pt_lt1); |
| 128 | Eigen2Point(q * (m_scale * lt2) + p, pt_lt2); |
| 129 | Eigen2Point(q * (m_scale * oc) + p, pt_oc); |
| 130 | |
| 131 | // image boundaries |
| 132 | marker.points.push_back(pt_lt); |
| 133 | marker.points.push_back(pt_lb); |
| 134 | marker.colors.push_back(m_image_boundary_color); |
| 135 | marker.colors.push_back(m_image_boundary_color); |
| 136 | |
| 137 | marker.points.push_back(pt_lb); |
| 138 | marker.points.push_back(pt_rb); |
| 139 | marker.colors.push_back(m_image_boundary_color); |
| 140 | marker.colors.push_back(m_image_boundary_color); |
| 141 | |
| 142 | marker.points.push_back(pt_rb); |
| 143 | marker.points.push_back(pt_rt); |
| 144 | marker.colors.push_back(m_image_boundary_color); |
| 145 | marker.colors.push_back(m_image_boundary_color); |
| 146 | |
| 147 | marker.points.push_back(pt_rt); |
| 148 | marker.points.push_back(pt_lt); |
| 149 | marker.colors.push_back(m_image_boundary_color); |
| 150 | marker.colors.push_back(m_image_boundary_color); |
| 151 | |
| 152 | // top-left indicator |
| 153 | marker.points.push_back(pt_lt0); |
| 154 | marker.points.push_back(pt_lt1); |
| 155 | marker.colors.push_back(m_image_boundary_color); |
| 156 | marker.colors.push_back(m_image_boundary_color); |
| 157 | |
| 158 | marker.points.push_back(pt_lt1); |
| 159 | marker.points.push_back(pt_lt2); |
| 160 | marker.colors.push_back(m_image_boundary_color); |
no test coverage detected