ROSVisualizerHelper.cpp
Go to the documentation of this file.
1 /*
2  * OpenVINS: An Open Platform for Visual-Inertial Research
3  * Copyright (C) 2018-2023 Patrick Geneva
4  * Copyright (C) 2018-2023 Guoquan Huang
5  * Copyright (C) 2018-2023 OpenVINS Contributors
6  * Copyright (C) 2018-2019 Kevin Eckenhoff
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, either version 3 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program. If not, see <https://www.gnu.org/licenses/>.
20  */
21 
22 #include "ROSVisualizerHelper.h"
23 
24 #include "core/VioManager.h"
25 #include "sim/Simulator.h"
26 #include "state/State.h"
27 #include "state/StateHelper.h"
28 
29 #include "types/PoseJPL.h"
30 
31 using namespace ov_msckf;
32 using namespace std;
33 
34 #if ROS_AVAILABLE == 1
35 sensor_msgs::PointCloud2 ROSVisualizerHelper::get_ros_pointcloud(const std::vector<Eigen::Vector3d> &feats) {
36 
37  // Declare message and sizes
38  sensor_msgs::PointCloud2 cloud;
39  cloud.header.frame_id = "global";
40  cloud.header.stamp = ros::Time::now();
41  cloud.width = feats.size();
42  cloud.height = 1;
43  cloud.is_bigendian = false;
44  cloud.is_dense = false; // there may be invalid points
45 
46  // Setup pointcloud fields
47  sensor_msgs::PointCloud2Modifier modifier(cloud);
48  modifier.setPointCloud2FieldsByString(1, "xyz");
49  modifier.resize(feats.size());
50 
51  // Iterators
52  sensor_msgs::PointCloud2Iterator<float> out_x(cloud, "x");
53  sensor_msgs::PointCloud2Iterator<float> out_y(cloud, "y");
54  sensor_msgs::PointCloud2Iterator<float> out_z(cloud, "z");
55 
56  // Fill our iterators
57  for (const auto &pt : feats) {
58  *out_x = (float)pt(0);
59  ++out_x;
60  *out_y = (float)pt(1);
61  ++out_y;
62  *out_z = (float)pt(2);
63  ++out_z;
64  }
65 
66  return cloud;
67 }
68 
69 tf::StampedTransform ROSVisualizerHelper::get_stamped_transform_from_pose(const std::shared_ptr<ov_type::PoseJPL> &pose, bool flip_trans) {
70 
71  // Need to flip the transform to the IMU frame
72  Eigen::Vector4d q_ItoC = pose->quat();
73  Eigen::Vector3d p_CinI = pose->pos();
74  if (flip_trans) {
75  p_CinI = -pose->Rot().transpose() * pose->pos();
76  }
77 
78  // publish our transform on TF
79  // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish
80  // NOTE: a rotation from ItoC in JPL has the same xyzw as a CtoI Hamilton rotation
81  tf::StampedTransform trans;
82  trans.stamp_ = ros::Time::now();
83  tf::Quaternion quat(q_ItoC(0), q_ItoC(1), q_ItoC(2), q_ItoC(3));
84  trans.setRotation(quat);
85  tf::Vector3 orig(p_CinI(0), p_CinI(1), p_CinI(2));
86  trans.setOrigin(orig);
87  return trans;
88 }
89 #endif
90 
91 #if ROS_AVAILABLE == 2
92 sensor_msgs::msg::PointCloud2 ROSVisualizerHelper::get_ros_pointcloud(std::shared_ptr<rclcpp::Node> node,
93  const std::vector<Eigen::Vector3d> &feats) {
94 
95  // Declare message and sizes
96  sensor_msgs::msg::PointCloud2 cloud;
97  cloud.header.frame_id = "global";
98  cloud.header.stamp = node->now();
99  cloud.width = feats.size();
100  cloud.height = 1;
101  cloud.is_bigendian = false;
102  cloud.is_dense = false; // there may be invalid points
103 
104  // Setup pointcloud fields
105  sensor_msgs::PointCloud2Modifier modifier(cloud);
106  modifier.setPointCloud2FieldsByString(1, "xyz");
107  modifier.resize(feats.size());
108 
109  // Iterators
110  sensor_msgs::PointCloud2Iterator<float> out_x(cloud, "x");
111  sensor_msgs::PointCloud2Iterator<float> out_y(cloud, "y");
112  sensor_msgs::PointCloud2Iterator<float> out_z(cloud, "z");
113 
114  // Fill our iterators
115  for (const auto &pt : feats) {
116  *out_x = (float)pt(0);
117  ++out_x;
118  *out_y = (float)pt(1);
119  ++out_y;
120  *out_z = (float)pt(2);
121  ++out_z;
122  }
123 
124  return cloud;
125 }
126 
127 geometry_msgs::msg::TransformStamped ROSVisualizerHelper::get_stamped_transform_from_pose(std::shared_ptr<rclcpp::Node> node,
128  const std::shared_ptr<ov_type::PoseJPL> &pose,
129  bool flip_trans) {
130 
131  // Need to flip the transform to the IMU frame
132  Eigen::Vector4d q_ItoC = pose->quat();
133  Eigen::Vector3d p_CinI = pose->pos();
134  if (flip_trans) {
135  p_CinI = -pose->Rot().transpose() * pose->pos();
136  }
137 
138  // publish our transform on TF
139  // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish
140  // NOTE: a rotation from ItoC in JPL has the same xyzw as a CtoI Hamilton rotation
141  geometry_msgs::msg::TransformStamped trans;
142  trans.header.stamp = node->now();
143  trans.transform.rotation.x = q_ItoC(0);
144  trans.transform.rotation.y = q_ItoC(1);
145  trans.transform.rotation.z = q_ItoC(2);
146  trans.transform.rotation.w = q_ItoC(3);
147  trans.transform.translation.x = p_CinI(0);
148  trans.transform.translation.y = p_CinI(1);
149  trans.transform.translation.z = p_CinI(2);
150  return trans;
151 }
152 #endif
153 
154 void ROSVisualizerHelper::sim_save_total_state_to_file(std::shared_ptr<State> state, std::shared_ptr<Simulator> sim,
155  std::ofstream &of_state_est, std::ofstream &of_state_std,
156  std::ofstream &of_state_gt) {
157 
158  // We want to publish in the IMU clock frame
159  // The timestamp in the state will be the last camera time
160  double t_ItoC = state->_calib_dt_CAMtoIMU->value()(0);
161  double timestamp_inI = state->_timestamp + t_ItoC;
162 
163  // If we have our simulator, then save it to our groundtruth file
164  if (sim != nullptr) {
165 
166  // Note that we get the true time in the IMU clock frame
167  // NOTE: we record both the estimate and groundtruth with the same "true" timestamp if we are doing simulation
168  Eigen::Matrix<double, 17, 1> state_gt;
169  timestamp_inI = state->_timestamp + sim->get_true_parameters().calib_camimu_dt;
170  if (sim->get_state(timestamp_inI, state_gt)) {
171  // STATE: write current true state
172  of_state_gt.precision(5);
173  of_state_gt.setf(std::ios::fixed, std::ios::floatfield);
174  of_state_gt << state_gt(0) << " ";
175  of_state_gt.precision(6);
176  of_state_gt << state_gt(1) << " " << state_gt(2) << " " << state_gt(3) << " " << state_gt(4) << " ";
177  of_state_gt << state_gt(5) << " " << state_gt(6) << " " << state_gt(7) << " ";
178  of_state_gt << state_gt(8) << " " << state_gt(9) << " " << state_gt(10) << " ";
179  of_state_gt << state_gt(11) << " " << state_gt(12) << " " << state_gt(13) << " ";
180  of_state_gt << state_gt(14) << " " << state_gt(15) << " " << state_gt(16) << " ";
181 
182  // TIMEOFF: Get the current true time offset
183  of_state_gt.precision(7);
184  of_state_gt << sim->get_true_parameters().calib_camimu_dt << " ";
185  of_state_gt.precision(0);
186  of_state_gt << state->_options.num_cameras << " ";
187  of_state_gt.precision(6);
188 
189  // CALIBRATION: Write the camera values to file
190  assert(state->_options.num_cameras == sim->get_true_parameters().state_options.num_cameras);
191  for (int i = 0; i < state->_options.num_cameras; i++) {
192  // Intrinsics values
193  of_state_gt << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(0) << " "
194  << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(1) << " "
195  << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(2) << " "
196  << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(3) << " ";
197  of_state_gt << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(4) << " "
198  << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(5) << " "
199  << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(6) << " "
200  << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(7) << " ";
201  // Rotation and position
202  of_state_gt << sim->get_true_parameters().camera_extrinsics.at(i)(0) << " " << sim->get_true_parameters().camera_extrinsics.at(i)(1)
203  << " " << sim->get_true_parameters().camera_extrinsics.at(i)(2) << " "
204  << sim->get_true_parameters().camera_extrinsics.at(i)(3) << " ";
205  of_state_gt << sim->get_true_parameters().camera_extrinsics.at(i)(4) << " " << sim->get_true_parameters().camera_extrinsics.at(i)(5)
206  << " " << sim->get_true_parameters().camera_extrinsics.at(i)(6) << " ";
207  }
208 
209  // Get the base IMU information
210  of_state_gt.precision(0);
211  of_state_gt << sim->get_true_parameters().state_options.imu_model << " ";
212  of_state_gt.precision(8);
213 
214  of_state_gt << sim->get_true_parameters().vec_dw(0) << " " << sim->get_true_parameters().vec_dw(1) << " "
215  << sim->get_true_parameters().vec_dw(2) << " " << sim->get_true_parameters().vec_dw(3) << " "
216  << sim->get_true_parameters().vec_dw(4) << " " << sim->get_true_parameters().vec_dw(5) << " ";
217  of_state_gt << sim->get_true_parameters().vec_da(0) << " " << sim->get_true_parameters().vec_da(1) << " "
218  << sim->get_true_parameters().vec_da(2) << " " << sim->get_true_parameters().vec_da(3) << " "
219  << sim->get_true_parameters().vec_da(4) << " " << sim->get_true_parameters().vec_da(5) << " ";
220  of_state_gt << sim->get_true_parameters().vec_tg(0) << " " << sim->get_true_parameters().vec_tg(1) << " "
221  << sim->get_true_parameters().vec_tg(2) << " " << sim->get_true_parameters().vec_tg(3) << " "
222  << sim->get_true_parameters().vec_tg(4) << " " << sim->get_true_parameters().vec_tg(5) << " "
223  << sim->get_true_parameters().vec_tg(6) << " " << sim->get_true_parameters().vec_tg(7) << " "
224  << sim->get_true_parameters().vec_tg(8) << " ";
225  of_state_gt << sim->get_true_parameters().q_GYROtoIMU(0) << " " << sim->get_true_parameters().q_GYROtoIMU(1) << " "
226  << sim->get_true_parameters().q_GYROtoIMU(2) << " " << sim->get_true_parameters().q_GYROtoIMU(3) << " ";
227  of_state_gt << sim->get_true_parameters().q_ACCtoIMU(0) << " " << sim->get_true_parameters().q_ACCtoIMU(1) << " "
228  << sim->get_true_parameters().q_ACCtoIMU(2) << " " << sim->get_true_parameters().q_ACCtoIMU(3) << " ";
229 
230  // New line
231  of_state_gt << endl;
232  }
233  }
234 
235  //==========================================================================
236  //==========================================================================
237  //==========================================================================
238 
239  // Get the covariance of the whole system
240  Eigen::MatrixXd cov = StateHelper::get_full_covariance(state);
241 
242  // STATE: Write the current state to file
243  of_state_est.precision(5);
244  of_state_est.setf(std::ios::fixed, std::ios::floatfield);
245  of_state_est << timestamp_inI << " ";
246  of_state_est.precision(6);
247  of_state_est << state->_imu->quat()(0) << " " << state->_imu->quat()(1) << " " << state->_imu->quat()(2) << " " << state->_imu->quat()(3)
248  << " ";
249  of_state_est << state->_imu->pos()(0) << " " << state->_imu->pos()(1) << " " << state->_imu->pos()(2) << " ";
250  of_state_est << state->_imu->vel()(0) << " " << state->_imu->vel()(1) << " " << state->_imu->vel()(2) << " ";
251  of_state_est << state->_imu->bias_g()(0) << " " << state->_imu->bias_g()(1) << " " << state->_imu->bias_g()(2) << " ";
252  of_state_est << state->_imu->bias_a()(0) << " " << state->_imu->bias_a()(1) << " " << state->_imu->bias_a()(2) << " ";
253 
254  // STATE: Write current uncertainty to file
255  of_state_std.precision(5);
256  of_state_std.setf(std::ios::fixed, std::ios::floatfield);
257  of_state_std << timestamp_inI << " ";
258  of_state_std.precision(6);
259  int id = state->_imu->q()->id();
260  of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " ";
261  id = state->_imu->p()->id();
262  of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " ";
263  id = state->_imu->v()->id();
264  of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " ";
265  id = state->_imu->bg()->id();
266  of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " ";
267  id = state->_imu->ba()->id();
268  of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " ";
269 
270  // TIMEOFF: Get the current estimate time offset
271  of_state_est.precision(7);
272  of_state_est << state->_calib_dt_CAMtoIMU->value()(0) << " ";
273  of_state_est.precision(0);
274  of_state_est << state->_options.num_cameras << " ";
275  of_state_est.precision(6);
276 
277  // TIMEOFF: Get the current std values
278  if (state->_options.do_calib_camera_timeoffset) {
279  of_state_std << std::sqrt(cov(state->_calib_dt_CAMtoIMU->id(), state->_calib_dt_CAMtoIMU->id())) << " ";
280  } else {
281  of_state_std << 0.0 << " ";
282  }
283  of_state_std.precision(0);
284  of_state_std << state->_options.num_cameras << " ";
285  of_state_std.precision(6);
286 
287  // CALIBRATION: Write the camera values to file
288  for (int i = 0; i < state->_options.num_cameras; i++) {
289  // Intrinsics values
290  of_state_est << state->_cam_intrinsics.at(i)->value()(0) << " " << state->_cam_intrinsics.at(i)->value()(1) << " "
291  << state->_cam_intrinsics.at(i)->value()(2) << " " << state->_cam_intrinsics.at(i)->value()(3) << " ";
292  of_state_est << state->_cam_intrinsics.at(i)->value()(4) << " " << state->_cam_intrinsics.at(i)->value()(5) << " "
293  << state->_cam_intrinsics.at(i)->value()(6) << " " << state->_cam_intrinsics.at(i)->value()(7) << " ";
294  // Rotation and position
295  of_state_est << state->_calib_IMUtoCAM.at(i)->value()(0) << " " << state->_calib_IMUtoCAM.at(i)->value()(1) << " "
296  << state->_calib_IMUtoCAM.at(i)->value()(2) << " " << state->_calib_IMUtoCAM.at(i)->value()(3) << " ";
297  of_state_est << state->_calib_IMUtoCAM.at(i)->value()(4) << " " << state->_calib_IMUtoCAM.at(i)->value()(5) << " "
298  << state->_calib_IMUtoCAM.at(i)->value()(6) << " ";
299  // Covariance
300  if (state->_options.do_calib_camera_intrinsics) {
301  int index_in = state->_cam_intrinsics.at(i)->id();
302  of_state_std << std::sqrt(cov(index_in + 0, index_in + 0)) << " " << std::sqrt(cov(index_in + 1, index_in + 1)) << " "
303  << std::sqrt(cov(index_in + 2, index_in + 2)) << " " << std::sqrt(cov(index_in + 3, index_in + 3)) << " ";
304  of_state_std << std::sqrt(cov(index_in + 4, index_in + 4)) << " " << std::sqrt(cov(index_in + 5, index_in + 5)) << " "
305  << std::sqrt(cov(index_in + 6, index_in + 6)) << " " << std::sqrt(cov(index_in + 7, index_in + 7)) << " ";
306  } else {
307  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " ";
308  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " ";
309  }
310  if (state->_options.do_calib_camera_pose) {
311  int index_ex = state->_calib_IMUtoCAM.at(i)->id();
312  of_state_std << std::sqrt(cov(index_ex + 0, index_ex + 0)) << " " << std::sqrt(cov(index_ex + 1, index_ex + 1)) << " "
313  << std::sqrt(cov(index_ex + 2, index_ex + 2)) << " ";
314  of_state_std << std::sqrt(cov(index_ex + 3, index_ex + 3)) << " " << std::sqrt(cov(index_ex + 4, index_ex + 4)) << " "
315  << std::sqrt(cov(index_ex + 5, index_ex + 5)) << " ";
316  } else {
317  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " ";
318  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " ";
319  }
320  }
321 
322  // imu intrinsics: what model we are using
323  of_state_est.precision(0);
324  of_state_est << state->_options.imu_model << " ";
325  of_state_est.precision(8);
326  of_state_std.precision(0);
327  of_state_std << state->_options.imu_model << " ";
328  of_state_std.precision(8);
329 
330  // imu intrinsics: dw
331  of_state_est << state->_calib_imu_dw->value()(0) << " " << state->_calib_imu_dw->value()(1) << " " << state->_calib_imu_dw->value()(2)
332  << " " << state->_calib_imu_dw->value()(3) << " " << state->_calib_imu_dw->value()(4) << " "
333  << state->_calib_imu_dw->value()(5) << " ";
334  if (state->_options.do_calib_imu_intrinsics) {
335  int index_dw = state->_calib_imu_dw->id();
336  of_state_std << std::sqrt(cov(index_dw + 0, index_dw + 0)) << " " << std::sqrt(cov(index_dw + 1, index_dw + 1)) << " "
337  << std::sqrt(cov(index_dw + 2, index_dw + 2)) << " " << std::sqrt(cov(index_dw + 3, index_dw + 3)) << " "
338  << std::sqrt(cov(index_dw + 4, index_dw + 4)) << " " << std::sqrt(cov(index_dw + 5, index_dw + 5)) << " ";
339  } else {
340  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " ";
341  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " ";
342  }
343 
344  // imu intrinsics: da
345  of_state_est << state->_calib_imu_da->value()(0) << " " << state->_calib_imu_da->value()(1) << " " << state->_calib_imu_da->value()(2)
346  << " " << state->_calib_imu_da->value()(3) << " " << state->_calib_imu_da->value()(4) << " "
347  << state->_calib_imu_da->value()(5) << " ";
348  if (state->_options.do_calib_imu_intrinsics) {
349  int index_da = state->_calib_imu_da->id();
350  of_state_std << std::sqrt(cov(index_da + 0, index_da + 0)) << " " << std::sqrt(cov(index_da + 1, index_da + 1)) << " "
351  << std::sqrt(cov(index_da + 2, index_da + 2)) << " " << std::sqrt(cov(index_da + 3, index_da + 3)) << " "
352  << std::sqrt(cov(index_da + 4, index_da + 4)) << " " << std::sqrt(cov(index_da + 5, index_da + 5)) << " ";
353  } else {
354  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " ";
355  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " ";
356  }
357 
358  // imu intrinsics: tg
359  of_state_est << state->_calib_imu_tg->value()(0) << " " << state->_calib_imu_tg->value()(1) << " " << state->_calib_imu_tg->value()(2)
360  << " " << state->_calib_imu_tg->value()(3) << " " << state->_calib_imu_tg->value()(4) << " "
361  << state->_calib_imu_tg->value()(5) << " " << state->_calib_imu_tg->value()(6) << " " << state->_calib_imu_tg->value()(7)
362  << " " << state->_calib_imu_tg->value()(8) << " ";
363  if (state->_options.do_calib_imu_intrinsics && state->_options.do_calib_imu_g_sensitivity) {
364  int index_tg = state->_calib_imu_tg->id();
365  of_state_std << std::sqrt(cov(index_tg + 0, index_tg + 0)) << " " << std::sqrt(cov(index_tg + 1, index_tg + 1)) << " "
366  << std::sqrt(cov(index_tg + 2, index_tg + 2)) << " " << std::sqrt(cov(index_tg + 3, index_tg + 3)) << " "
367  << std::sqrt(cov(index_tg + 4, index_tg + 4)) << " " << std::sqrt(cov(index_tg + 5, index_tg + 5)) << " "
368  << std::sqrt(cov(index_tg + 6, index_tg + 6)) << " " << std::sqrt(cov(index_tg + 7, index_tg + 7)) << " "
369  << std::sqrt(cov(index_tg + 8, index_tg + 8)) << " ";
370  } else {
371  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " ";
372  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " ";
373  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " ";
374  }
375 
376  // imu intrinsics: kalibr R_gyrotoI
377  of_state_est << state->_calib_imu_GYROtoIMU->value()(0) << " " << state->_calib_imu_GYROtoIMU->value()(1) << " "
378  << state->_calib_imu_GYROtoIMU->value()(2) << " " << state->_calib_imu_GYROtoIMU->value()(3) << " ";
379  if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == StateOptions::ImuModel::KALIBR) {
380  int index_wtoI = state->_calib_imu_GYROtoIMU->id();
381  of_state_std << std::sqrt(cov(index_wtoI + 0, index_wtoI + 0)) << " " << std::sqrt(cov(index_wtoI + 1, index_wtoI + 1)) << " "
382  << std::sqrt(cov(index_wtoI + 2, index_wtoI + 2)) << " " << std::sqrt(cov(index_wtoI + 3, index_wtoI + 3)) << " ";
383  } else {
384  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " ";
385  }
386 
387  // imu intrinsics: rpng R_acctoI
388  of_state_est << state->_calib_imu_ACCtoIMU->value()(0) << " " << state->_calib_imu_ACCtoIMU->value()(1) << " "
389  << state->_calib_imu_ACCtoIMU->value()(2) << " " << state->_calib_imu_ACCtoIMU->value()(3) << " ";
390  if (state->_options.do_calib_imu_intrinsics && state->_options.imu_model == StateOptions::ImuModel::RPNG) {
391  int index_atoI = state->_calib_imu_ACCtoIMU->id();
392  of_state_std << std::sqrt(cov(index_atoI + 0, index_atoI + 0)) << " " << std::sqrt(cov(index_atoI + 1, index_atoI + 1)) << " "
393  << std::sqrt(cov(index_atoI + 2, index_atoI + 2)) << " " << std::sqrt(cov(index_atoI + 3, index_atoI + 3)) << " ";
394  } else {
395  of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " ";
396  }
397 
398  // Done with the estimates!
399  of_state_est << endl;
400  of_state_std << endl;
401 }
tf::StampedTransform::stamp_
ros::Time stamp_
ROSVisualizerHelper.h
ov_msckf
Extended Kalman Filter estimator.
Definition: VioManager.h:46
tf::StampedTransform
StateHelper.h
sensor_msgs::PointCloud2Iterator
PoseJPL.h
tf::Transform::setRotation
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
VioManager.h
tf::Transform::setOrigin
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
std
sensor_msgs::PointCloud2Modifier
sim
std::shared_ptr< Simulator > sim
Definition: run_simulation.cpp:42
State.h
Simulator.h
ov_msckf::ROSVisualizerHelper::sim_save_total_state_to_file
static void sim_save_total_state_to_file(std::shared_ptr< State > state, std::shared_ptr< Simulator > sim, std::ofstream &of_state_est, std::ofstream &of_state_std, std::ofstream &of_state_gt)
Save current estimate state and groundtruth including calibration.
Definition: ROSVisualizerHelper.cpp:154
ov_msckf::StateHelper::get_full_covariance
static Eigen::MatrixXd get_full_covariance(std::shared_ptr< State > state)
This gets the full covariance matrix.
Definition: StateHelper.cpp:256
tf::Quaternion
ros::Time::now
static Time now()

ov_msckf
Author(s): Patrick Geneva , Kevin Eckenhoff , Guoquan Huang
autogenerated on Mon Dec 16 2024 03:06:54