11 #ifndef VISIONSENSORCLASS_H
12 #define VISIONSENSORCLASS_H
37 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 VisionSensorClass(
const std::string& name, std::shared_ptr<CoreState> core_states,
bool update_scale =
true)
52 std::cout <<
"Created: [" << this->
name_ <<
"] Sensor" << std::endl;
76 std::shared_ptr<CoreType> latest_core_data)
81 std::string calibration_type;
85 calibration_type =
"Given";
100 Eigen::Quaterniond q_wi(latest_core_data->state_.q_wi_);
101 Eigen::Quaterniond q_ic(calib.
state_.q_ic_);
103 Eigen::Quaterniond q_vw = (q_wi * q_ic * q_vc.inverse()).inverse();
105 Eigen::Matrix3d R_wi(q_wi.toRotationMatrix());
106 Eigen::Matrix3d R_ic(q_ic.toRotationMatrix());
108 Eigen::Matrix3d R_vw(q_vw.toRotationMatrix());
110 Eigen::Vector3d p_wi(latest_core_data->state_.p_wi_);
111 Eigen::Vector3d p_ic(calib.
state_.p_ic_);
112 Eigen::Vector3d p_vc(measurement.
position_);
114 Eigen::Vector3d p_vw = -(R_vw * (p_wi + (R_wi * p_ic)) - p_vc);
116 sensor_state.
state_.q_vw_ = q_vw;
117 sensor_state.
state_.p_vw_ = p_vw;
119 std::cout <<
"Info: [" <<
name_ <<
"] Reference Frame initialized to:" << std::endl;
120 std::cout <<
"\tP_vw[m]: [" << sensor_state.
state_.p_vw_.transpose() <<
" ]" << std::endl;
122 Eigen::Vector4d q_vw_out(sensor_state.
state_.q_vw_.w(), sensor_state.
state_.q_vw_.x(),
123 sensor_state.
state_.q_vw_.y(), sensor_state.
state_.q_vw_.z());
125 std::cout <<
"\tq_vw: [" << q_vw_out.transpose() <<
" ]" << std::endl;
126 std::cout <<
"\tR_vw[deg]: ["
127 << sensor_state.
state_.q_vw_.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * (180 / M_PI) <<
" ]"
132 calibration_type =
"Auto";
155 std::cout <<
"Vision calibration AUTO init not implemented yet" << std::endl;
160 BufferDataType result(std::make_shared<CoreType>(*latest_core_data.get()),
161 std::make_shared<VisionSensorData>(sensor_state));
165 std::cout <<
"Info: Initialized [" <<
name_ <<
"] with [" << calibration_type <<
"] Calibration at t=" << timestamp
170 std::cout <<
"Info: [" <<
name_ <<
"] Calibration(rounded):" << std::endl;
171 std::cout <<
"\tP_vw[m]: [" << sensor_state.
state_.p_vw_.transpose() <<
" ]" << std::endl;
172 std::cout <<
"\tR_vw[deg]: ["
173 << sensor_state.
state_.q_vw_.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * (180 / M_PI) <<
" ]"
175 std::cout <<
"\tP_ic[m]: [" << sensor_state.
state_.p_ic_.transpose() <<
" ]" << std::endl;
176 std::cout <<
"\tR_ic[deg]: ["
177 << sensor_state.
state_.q_ic_.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * (180 / M_PI) <<
" ]"
179 std::cout <<
"\tLambda[%]: [" << sensor_state.
state_.lambda_ * 100 <<
" ]" << std::endl;
186 std::shared_ptr<void> latest_sensor_data,
const Eigen::MatrixXd& prior_cov,
200 Eigen::Vector3d p_meas = meas->
position_;
208 Eigen::MatrixXd R_meas_dyn;
215 R_meas_dyn = this->
R_.asDiagonal();
217 const Eigen::Matrix<double, 6, 6> R_meas = R_meas_dyn;
220 const int size_of_sensor_state = prior_sensor_state.
cov_size_;
221 const int size_of_full_error_state = size_of_core_state + size_of_sensor_state;
222 const Eigen::MatrixXd P = prior_cov;
223 assert(P.size() == size_of_full_error_state * size_of_full_error_state);
226 const Eigen::Matrix3d I_3 = Eigen::Matrix3d::Identity();
227 const Eigen::Vector3d P_wi = prior_core_state.
p_wi_;
228 const Eigen::Matrix3d R_wi = prior_core_state.
q_wi_.toRotationMatrix();
229 const Eigen::Vector3d P_vw = prior_sensor_state.
p_vw_;
230 const Eigen::Matrix3d R_vw = prior_sensor_state.
q_vw_.toRotationMatrix();
231 const Eigen::Vector3d P_ic = prior_sensor_state.
p_ic_;
232 const Eigen::Matrix3d R_ic = prior_sensor_state.
q_ic_.toRotationMatrix();
233 const double L = prior_sensor_state.
lambda_;
236 const Eigen::Matrix3d Hp_pwi = L * R_vw;
237 const Eigen::Matrix3d Hp_vwi = Eigen::Matrix3d::Zero();
238 const Eigen::Matrix3d Hp_rwi = -L * R_vw * R_wi *
Utils::Skew(P_ic);
239 const Eigen::Matrix3d Hp_bw = Eigen::Matrix3d::Zero();
240 const Eigen::Matrix3d Hp_ba = Eigen::Matrix3d::Zero();
242 const Eigen::Matrix3d Hp_pvw = I_3 * L;
243 const Eigen::Matrix3d Hp_rvw = -L * R_vw *
Utils::Skew(P_wi + R_wi * P_ic);
244 const Eigen::Matrix3d Hp_pic = L * R_vw * R_wi;
245 const Eigen::Matrix3d Hp_ric = Eigen::Matrix3d::Zero();
246 Eigen::Vector3d Hp_lambda;
249 Hp_lambda = P_vw + R_vw * (P_wi + R_wi * P_ic);
253 Hp_lambda = Eigen::Vector3d::Zero();
257 Eigen::MatrixXd H_p(3, Hp_pwi.cols() + Hp_vwi.cols() + Hp_rwi.cols() + Hp_bw.cols() + Hp_ba.cols() + Hp_pvw.cols() +
258 Hp_rvw.cols() + Hp_pic.cols() + Hp_ric.cols() + Hp_lambda.cols());
260 H_p << Hp_pwi, Hp_vwi, Hp_rwi, Hp_bw, Hp_ba, Hp_pvw, Hp_rvw, Hp_pic, Hp_ric, Hp_lambda;
263 const Eigen::Matrix3d Hr_pwi = Eigen::Matrix3d::Zero();
264 const Eigen::Matrix3d Hr_vwi = Eigen::Matrix3d::Zero();
265 const Eigen::Matrix3d Hr_rwi = R_ic.transpose();
266 const Eigen::Matrix3d Hr_bw = Eigen::Matrix3d::Zero();
267 const Eigen::Matrix3d Hr_ba = Eigen::Matrix3d::Zero();
269 const Eigen::Matrix3d Hr_pvw = Eigen::Matrix3d::Zero();
270 const Eigen::Matrix3d Hr_rvw = R_ic.transpose() * R_wi.transpose();
271 const Eigen::Matrix3d Hr_pic = Eigen::Matrix3d::Zero();
272 const Eigen::Matrix3d Hr_ric = I_3;
273 const Eigen::Vector3d Hr_lambda = Eigen::Vector3d::Zero();
277 Eigen::MatrixXd H_r(3, Hr_pwi.cols() + Hr_vwi.cols() + Hr_rwi.cols() + Hr_bw.cols() + Hr_ba.cols() + Hr_pvw.cols() +
278 Hr_rvw.cols() + Hr_pic.cols() + Hr_ric.cols() + Hr_lambda.cols());
279 H_r << Hr_pwi, Hr_vwi, Hr_rwi, Hr_bw, Hr_ba, Hr_pvw, Hr_rvw, Hr_pic, Hr_ric, Hr_lambda;
282 Eigen::MatrixXd H(H_p.rows() + H_r.rows(), H_r.cols());
287 const Eigen::Vector3d p_est = (P_vw + R_vw * (P_wi + R_wi * P_ic)) * L;
288 const Eigen::Vector3d res_p = p_meas - p_est;
291 const Eigen::Quaternion<double> q_est =
292 prior_sensor_state.
q_vw_ * prior_core_state.
q_wi_ * prior_sensor_state.
q_ic_;
293 const Eigen::Quaternion<double> res_q = q_est.inverse() * q_meas;
294 const Eigen::Vector3d res_r = 2 * res_q.vec() / res_q.w();
297 residual_ = Eigen::MatrixXd(res_p.rows() + res_r.rows(), 1);
303 assert(correction.size() == size_of_full_error_state * 1);
313 assert(P_updated.size() == size_of_full_error_state * size_of_full_error_state);
321 const Eigen::MatrixXd sensor_correction = correction.block(size_of_core_state, 0, size_of_sensor_state, 1);
328 core_data.
state_ = corrected_core_state;
331 std::shared_ptr<VisionSensorData> sensor_data(std::make_shared<VisionSensorData>());
332 sensor_data->set_cov(P_updated);
333 sensor_data->state_ = corrected_sensor_state;
335 BufferDataType state_entry(std::make_shared<CoreType>(core_data), sensor_data);
346 *new_state_data = state_entry;
352 const Eigen::MatrixXd& correction)
364 corrected_sensor_state.
p_vw_ = prior_sensor_state.
p_vw_ + correction.block(0, 0, 3, 1);
365 corrected_sensor_state.
q_vw_ =
368 corrected_sensor_state.
p_ic_ = prior_sensor_state.
p_ic_ + correction.block(6, 0, 3, 1);
369 corrected_sensor_state.
q_ic_ =
372 corrected_sensor_state.
lambda_ = prior_sensor_state.
lambda_ + correction(12);
374 return corrected_sensor_state;
bool has_meas_noise
Definition: measurement_base_class.h:23
bool get_meas_noise(Eigen::MatrixXd *meas_noise)
get the measurement noise associated with the current sensor measurement
Definition: measurement_base_class.h:25
int cov_size_
Definition: base_states.h:25
The BaseSensorData class binds the sensor state and covariance matrix.
Definition: bind_sensor_data.h:29
EIGEN_MAKE_ALIGNED_OPERATOR_NEW T state_
Definition: bind_sensor_data.h:30
Eigen::MatrixXd get_full_cov() const
get_full_cov builds the full covariance matrix
Definition: bind_sensor_data.h:63
Eigen::MatrixXd sensor_cov_
covariance of the sensor states
Definition: bind_sensor_data.h:37
The BufferDataType binds the core and sensor state in form of a shared void pointer.
Definition: buffer_data_type.h:36
bool passed_
Determine if the test is performed or not.
Definition: ekf.h:84
bool do_test_
Upper critival value.
Definition: ekf.h:83
void set_dof(const int &value)
set_dof Set degree of freedom for the X2 distribution
void PrintReport(const std::string &name)
PrintReport Print a formated report e.g. if the test did not pass.
Definition: core_state_type.h:21
static constexpr int size_error_
Definition: core_state_type.h:38
Eigen::Vector3d p_wi_
Definition: core_state_type.h:27
Eigen::Quaternion< double > q_wi_
Definition: core_state_type.h:29
static CoreStateType ApplyCorrection(CoreStateType state_prior, Eigen::Matrix< double, CoreStateType::size_error_, 1 > correction)
ApplyCorrection.
Definition: core_state_type.h:46
Definition: core_type.h:19
CoreStateMatrix cov_
Definition: core_type.h:22
CoreStateType state_
Definition: core_type.h:21
Eigen::MatrixXd CalculateCovUpdate()
CalculateCovUpdate Updating the state covariance after the state update.
Eigen::MatrixXd CalculateCorrection()
Kalman gain.
std::string name_
Name of the individual sensor instance.
Definition: sensor_abs_class.h:23
bool is_initialized_
True if the sensor has been initialized.
Definition: sensor_abs_class.h:24
bool do_update_
True if updates should be performed with the sensor.
Definition: sensor_abs_class.h:25
bool const_ref_to_nav_
True if the reference should not be estimated.
Definition: sensor_abs_class.h:27
bool ref_to_nav_given_
True if the reference to the navigation frame is given by initial calibration.
Definition: sensor_abs_class.h:28
bool use_dynamic_meas_noise_
True if dynamic noise values from measurements should be used.
Definition: sensor_abs_class.h:29
Definition: update_sensor_abs_class.h:24
bool initial_calib_provided_
True if an initial calibration was provided.
Definition: update_sensor_abs_class.h:38
std::shared_ptr< CoreState > core_states_
Definition: update_sensor_abs_class.h:42
Eigen::VectorXd R_
Measurement noise "squared".
Definition: update_sensor_abs_class.h:32
std::shared_ptr< void > initial_calib_
Definition: update_sensor_abs_class.h:37
Chi2 chi2_
Definition: update_sensor_abs_class.h:40
Eigen::MatrixXd residual_
Definition: update_sensor_abs_class.h:31
static Eigen::MatrixXd EnforceMatrixSymmetry(const Eigen::Ref< const Eigen::MatrixXd > &mat_in)
EnforceMatrixSymmetry.
static Eigen::Matrix3d Skew(const Eigen::Vector3d &v)
skew generate the skew symmetric matrix of v
static Eigen::Quaterniond ApplySmallAngleQuatCorr(const Eigen::Quaterniond &q_prior, const Eigen::Vector3d &correction)
ApplySmallAngleQuatCorr.
Definition: vision_measurement_type.h:21
Eigen::Quaternion< double > orientation_
Quaternion [w x y z].
Definition: vision_measurement_type.h:26
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d position_
Position [x y z].
Definition: vision_measurement_type.h:25
Definition: vision_sensor_class.h:35
EIGEN_MAKE_ALIGNED_OPERATOR_NEW bool update_scale_
Definition: vision_sensor_class.h:39
Eigen::MatrixXd get_covariance(const std::shared_ptr< void > &sensor_data)
get_covariance Resolves a void pointer to the covariance matrix of the corresponding sensor type Each...
Definition: vision_sensor_class.h:63
VisionSensorClass(const std::string &name, std::shared_ptr< CoreState > core_states, bool update_scale=true)
Definition: vision_sensor_class.h:41
VisionSensorStateType get_state(const std::shared_ptr< void > &sensor_data)
Definition: vision_sensor_class.h:57
bool CalcUpdate(const Time &, std::shared_ptr< void > measurement, const CoreStateType &prior_core_state, std::shared_ptr< void > latest_sensor_data, const Eigen::MatrixXd &prior_cov, BufferDataType *new_state_data)
CalcUpdate Calculates the update for an individual sensor definition.
Definition: vision_sensor_class.h:185
void set_initial_calib(std::shared_ptr< void > calibration)
set_initial_calib Sets the calibration of an individual sensor
Definition: vision_sensor_class.h:69
VisionSensorStateType ApplyCorrection(const VisionSensorStateType &prior_sensor_state, const Eigen::MatrixXd &correction)
Definition: vision_sensor_class.h:351
BufferDataType Initialize(const Time ×tamp, std::shared_ptr< void > sensor_data, std::shared_ptr< CoreType > latest_core_data)
Initialize the state of an individual sensor.
Definition: vision_sensor_class.h:75
virtual ~VisionSensorClass()=default
Definition: vision_sensor_state_type.h:20
double lambda_
Definition: vision_sensor_state_type.h:28
Eigen::Quaternion< double > q_vw_
Definition: vision_sensor_state_type.h:25
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d p_vw_
Definition: vision_sensor_state_type.h:24
Eigen::Quaternion< double > q_ic_
Definition: vision_sensor_state_type.h:27
Eigen::Vector3d p_ic_
Definition: vision_sensor_state_type.h:26
Eigen::Matrix< double, CoreStateType::size_error_, 1 > CoreStateVector
Definition: core_state_type.h:135