Apollo 10.0
自动驾驶开放平台
类 | Public 成员函数 | 所有成员列表
apollo::perception::camera::TransformServer类 参考

#include <transform_server.h>

apollo::perception::camera::TransformServer 的协作图:

Public 成员函数

 TransformServer ()
 
 ~TransformServer ()
 
const std::set< std::string > & vertices ()
 
bool Init (const std::vector< std::string > &camera_names, const std::string &params_path)
 
bool AddTransform (const std::string &child_frame_id, const std::string &frame_id, const Eigen::Affine3d &transform)
 
bool QueryTransform (const std::string &child_frame_id, const std::string &frame_id, Eigen::Affine3d *transform)
 
void print ()
 
bool LoadFromFile (const std::string &tf_input, float frequency=200.0f)
 
bool QueryPos (double timestamp, Eigen::Affine3d *pose)
 

详细描述

在文件 transform_server.h 第 42 行定义.

构造及析构函数说明

◆ TransformServer()

apollo::perception::camera::TransformServer::TransformServer ( )
inline

在文件 transform_server.h 第 44 行定义.

44{}

◆ ~TransformServer()

apollo::perception::camera::TransformServer::~TransformServer ( )
inline

在文件 transform_server.h 第 45 行定义.

45{}

成员函数说明

◆ AddTransform()

bool apollo::perception::camera::TransformServer::AddTransform ( const std::string &  child_frame_id,
const std::string &  frame_id,
const Eigen::Affine3d &  transform 
)

在文件 transform_server.cc 第 143 行定义.

145 {
146 vertices_.insert(child_frame_id);
147 vertices_.insert(frame_id);
148
149 auto begin = edges_.lower_bound(child_frame_id);
150 auto end = edges_.upper_bound(child_frame_id);
151
152 for (auto iter = begin; iter != end; ++iter) {
153 if (iter->second.frame_id == frame_id) {
154 return false;
155 }
156 }
157
158 Edge e;
159 e.child_frame_id = child_frame_id;
160 e.frame_id = frame_id;
161 e.transform = transform;
162
163 Edge e_inv;
164 e_inv.child_frame_id = frame_id;
165 e_inv.frame_id = child_frame_id;
166 e_inv.transform = transform.inverse();
167 ADEBUG << "Add transform between " << frame_id << " and " << child_frame_id;
168 edges_.insert({child_frame_id, e});
169 edges_.insert({frame_id, e_inv});
170
171 return true;
172}
ADEBUG
#define ADEBUG
Definition log.h:41
apollo::drivers::robosense::frame_id
std::string frame_id
Point cloud frame id
Definition point_cloud_msg.h:51

◆ Init()

bool apollo::perception::camera::TransformServer::Init ( const std::vector< std::string > &  camera_names,
const std::string &  params_path 
)

在文件 transform_server.cc 第 26 行定义.

27 {
28 const std::string params_dir = params_path;
29 // 1. Init lidar height
30 try {
31 YAML::Node lidar_height =
32 YAML::LoadFile(params_dir + "/" + "velodyne128_height.yaml");
33 Eigen::Affine3d trans;
34 trans.linear() = Eigen::Matrix3d::Identity();
35 AINFO << trans.translation() << " "
36 << lidar_height["vehicle"]["parameters"]["height"];
37 trans.translation() << 0.0, 0.0,
38 lidar_height["vehicle"]["parameters"]["height"].as<double>();
39 AddTransform("velodyne128", "ground", trans);
40 } catch (YAML::InvalidNode &in) {
41 AERROR << "load velodyne128 extrisic file error"
42 << " YAML::InvalidNode exception";
43 return false;
44 } catch (YAML::TypedBadConversion<float> &bc) {
45 AERROR << "load velodyne128 extrisic file error, "
46 << "YAML::TypedBadConversion exception";
47 return false;
48 } catch (YAML::Exception &e) {
49 AERROR << "load velodyne128 extrisic file "
50 << " error, YAML exception:" << e.what();
51 return false;
52 }
53 // 2. Init lidar and camera extrinsic
54 std::vector<std::string> extrinsic_filelist;
55 extrinsic_filelist.push_back(params_dir +
56 "/velodyne128_novatel_extrinsics.yaml");
57 for (const auto &camera_name : camera_names) {
58 extrinsic_filelist.push_back(params_dir + "/" + camera_name +
59 "_extrinsics.yaml");
60 }
61
62 for (const auto &yaml_file : extrinsic_filelist) {
63 try {
64 YAML::Node node = YAML::LoadFile(yaml_file);
65 if (node.IsNull()) {
66 AINFO << "Load " << yaml_file << " failed! please check!";
67 return false;
68 }
69 std::string child_frame_id = node["child_frame_id"].as<std::string>();
70 std::string frame_id = node["header"]["frame_id"].as<std::string>();
71 double q[4] = {node["transform"]["rotation"]["w"].as<double>(),
72 node["transform"]["rotation"]["x"].as<double>(),
73 node["transform"]["rotation"]["y"].as<double>(),
74 node["transform"]["rotation"]["z"].as<double>()};
75 double t[3] = {node["transform"]["translation"]["x"].as<double>(),
76 node["transform"]["translation"]["y"].as<double>(),
77 node["transform"]["translation"]["z"].as<double>()};
78 Eigen::Quaterniond qq(q[0], q[1], q[2], q[3]);
79 Eigen::Affine3d trans;
80 trans.linear() = qq.matrix();
81 trans.translation() << t[0], t[1], t[2];
82 if (!AddTransform(child_frame_id, frame_id, trans)) {
83 AINFO << "failed to add transform from " << child_frame_id << " to "
84 << frame_id << std::endl;
85 }
86 } catch (YAML::InvalidNode &in) {
87 AERROR << "load camera extrisic file " << yaml_file
88 << " with error, YAML::InvalidNode exception";
89 return false;
90 } catch (YAML::TypedBadConversion<double> &bc) {
91 AERROR << "load camera extrisic file " << yaml_file
92 << " with error, YAML::TypedBadConversion exception";
93 return false;
94 } catch (YAML::Exception &e) {
95 AERROR << "load camera extrisic file " << yaml_file
96 << " with error, YAML exception:" << e.what();
97 return false;
98 }
99 }
100 return true;
101}
apollo::perception::camera::TransformServer::AddTransform
bool AddTransform(const std::string &child_frame_id, const std::string &frame_id, const Eigen::Affine3d &transform)
Definition transform_server.cc:143
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42

◆ LoadFromFile()

bool apollo::perception::camera::TransformServer::LoadFromFile ( const std::string &  tf_input,
float  frequency = 200.0f 
)

在文件 transform_server.cc 第 103 行定义.

104 {
105 if (frequency <= 0) {
106 AERROR << "Error frequency value:" << frequency;
107 return false;
108 }
109 std::ifstream fin(tf_input);
110 Transform tf;
111 int64_t ts;
112 while (fin >> ts) {
113 tf.timestamp = static_cast<double>(ts) * 1e-9;
114 fin >> tf.tx;
115 fin >> tf.ty;
116 fin >> tf.tz;
117 fin >> tf.qx;
118 fin >> tf.qy;
119 fin >> tf.qz;
120 fin >> tf.qw;
121 tf_.push_back(tf);
122 }
123 fin.close();
124 error_limit_ = 1 / frequency / 2.0f;
125 AINFO << "Load tf successfully. count: " << tf_.size()
126 << " error limit:" << error_limit_;
127 return true;
128}

◆ print()

void apollo::perception::camera::TransformServer::print ( )

在文件 transform_server.cc 第 235 行定义.

235 {
236 for (auto item : edges_) {
237 AINFO << "----------------" << std::endl;
238 AINFO << item.first << std::endl;
239 AINFO << "edge: " << std::endl;
240 AINFO << "from " << item.second.child_frame_id << " to "
241 << item.second.frame_id << std::endl;
242 Eigen::Affine3d trans = item.second.transform;
243 Eigen::Quaterniond quat(trans.linear());
244 AINFO << "rot: " << quat.x() << " " << quat.y() << " " << quat.z() << " "
245 << quat.w() << std::endl;
246 AINFO << "trans: " << trans.translation()[0] << " "
247 << trans.translation()[1] << " " << trans.translation()[2]
248 << std::endl;
249 }
250}

◆ QueryPos()

bool apollo::perception::camera::TransformServer::QueryPos ( double  timestamp,
Eigen::Affine3d *  pose 
)

在文件 transform_server.cc 第 130 行定义.

130 {
131 for (auto &&tf : tf_) {
132 if (Equal(timestamp, tf.timestamp, error_limit_)) {
133 Eigen::Quaterniond rotation(tf.qw, tf.qx, tf.qy, tf.qz);
134 pose->linear() = rotation.matrix();
135 pose->translation() << tf.tx, tf.ty, tf.tz;
136 AINFO << "Get Pose:\n" << pose->matrix();
137 return true;
138 }
139 }
140 return false;
141}
apollo::perception::camera::Equal
bool Equal(double x, double target, double eps)
Definition util.cc:24

◆ QueryTransform()

bool apollo::perception::camera::TransformServer::QueryTransform ( const std::string &  child_frame_id,
const std::string &  frame_id,
Eigen::Affine3d *  transform 
)

在文件 transform_server.cc 第 174 行定义.

176 {
177 *transform = Eigen::Affine3d::Identity();
178
179 if (child_frame_id == frame_id) {
180 return true;
181 }
182
183 // Vertices does not exist
184 if (vertices_.find(child_frame_id) == vertices_.end() ||
185 vertices_.find(frame_id) == vertices_.end()) {
186 return false;
187 }
188
189 std::map<std::string, bool> visited;
190 for (const auto &item : vertices_) {
191 visited[item] = false;
192 }
193
194 return FindTransform(child_frame_id, frame_id, transform, &visited);
195}

◆ vertices()

const std::set< std::string > & apollo::perception::camera::TransformServer::vertices ( )
inline

在文件 transform_server.h 第 47 行定义.

47{ return vertices_; }
该类的文档由以下文件生成: