33 buffer_ptr_ = apollo::transform::Buffer::Instance();
39 readers_.emplace_back(reader);
45 const std::shared_ptr<PointCloud>& point_cloud) {
46 auto target = std::make_shared<PointCloud>(*point_cloud);
47 auto fusion_readers = readers_;
50 && fusion_readers.size() > 0) {
51 for (
auto itr = fusion_readers.begin(); itr != fusion_readers.end();) {
53 if (!(*itr)->Empty()) {
54 auto source = (*itr)->GetLatestObserved();
58 Fusion(target, source);
59 itr = fusion_readers.erase(itr);
65 std::this_thread::sleep_for(std::chrono::milliseconds(5));
68 AINFO <<
"Pointcloud fusion diff: " << diff / 1000000 <<
"ms";
69 fusion_writer_->Write(target);
74bool PriSecFusionComponent::IsExpired(
75 const std::shared_ptr<PointCloud>& target,
76 const std::shared_ptr<PointCloud>& source) {
77 auto diff = target->measurement_time() - source->measurement_time();
81bool PriSecFusionComponent::QueryPoseAffine(
82 const std::string& target_frame_id,
83 const std::string& source_frame_id,
84 Eigen::Affine3d* pose) {
85 std::string err_string;
92 AERROR <<
"Can not find transform. "
93 <<
"target_id:" << target_frame_id
94 <<
" frame_id:" << source_frame_id
95 <<
" Error info: " << err_string;
102 }
catch (tf2::TransformException& ex) {
106 *pose = Eigen::Translation3d(
110 * Eigen::Quaterniond(
118void PriSecFusionComponent::AppendPointCloud(
119 std::shared_ptr<PointCloud> point_cloud,
120 std::shared_ptr<PointCloud> point_cloud_add,
121 const Eigen::Affine3d& pose) {
122 if (std::isnan(pose(0, 0))) {
123 for (
auto& point : point_cloud_add->point()) {
124 PointXYZIT* point_new = point_cloud->add_point();
125 point_new->set_intensity(point.intensity());
126 point_new->set_timestamp(point.timestamp());
127 point_new->set_x(point.x());
128 point_new->set_y(point.y());
129 point_new->set_z(point.z());
132 for (
auto& point : point_cloud_add->point()) {
133 if (std::isnan(point.x())) {
134 PointXYZIT* point_new = point_cloud->add_point();
135 point_new->set_intensity(point.intensity());
136 point_new->set_timestamp(point.timestamp());
137 point_new->set_x(point.x());
138 point_new->set_y(point.y());
139 point_new->set_z(point.z());
141 PointXYZIT* point_new = point_cloud->add_point();
142 point_new->set_intensity(point.intensity());
143 point_new->set_timestamp(point.timestamp());
144 Eigen::Matrix<float, 3, 1> pt(point.x(), point.y(), point.z());
145 point_new->set_x(
static_cast<float>(
146 pose(0, 0) * pt.coeffRef(0)
147 + pose(0, 1) * pt.coeffRef(1)
148 + pose(0, 2) * pt.coeffRef(2) + pose(0, 3)));
149 point_new->set_y(
static_cast<float>(
150 pose(1, 0) * pt.coeffRef(0)
151 + pose(1, 1) * pt.coeffRef(1)
152 + pose(1, 2) * pt.coeffRef(2) + pose(1, 3)));
153 point_new->set_z(
static_cast<float>(
154 pose(2, 0) * pt.coeffRef(0)
155 + pose(2, 1) * pt.coeffRef(1)
156 + pose(2, 2) * pt.coeffRef(2) + pose(2, 3)));
161 int new_width = point_cloud->point_size() / point_cloud->height();
162 point_cloud->set_width(new_width);
165bool PriSecFusionComponent::Fusion(
166 std::shared_ptr<PointCloud> target,
167 std::shared_ptr<PointCloud> source) {
168 Eigen::Affine3d pose;
170 target->header().frame_id(),
171 source->header().frame_id(),
173 AppendPointCloud(target, source, pose);