| 565 | } |
| 566 | |
| 567 | bool |
| 568 | Track3D::getPointXYZRGB(pcl::PointXYZRGB& p) |
| 569 | { |
| 570 | if(visibility_ == Track3D::NOT_VISIBLE) |
| 571 | return false; |
| 572 | |
| 573 | double _x, _y, _z; |
| 574 | filter_->getState(_x, _y, _z); |
| 575 | |
| 576 | p.x = float(_x); |
| 577 | p.y = float(_y); |
| 578 | p.z = float(_z); |
| 579 | uchar* rgb_ptr = (uchar*)&p.rgb; |
| 580 | *rgb_ptr++ = uchar(color_(0) * 255.0f); |
| 581 | *rgb_ptr++ = uchar(color_(1) * 255.0f); |
| 582 | *rgb_ptr++ = uchar(color_(2) * 255.0f); |
| 583 | return true; |
| 584 | } |
| 585 | |
| 586 | void |
| 587 | Track3D::toMsg(opt_msgs::Track3D& track_msg, bool vertical) |