mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

locationd: passing eigen objects by reference (#28719)

Browse Source 
pass eigen objects by reference
old-commit-hash: 3930ec9facb51491309ba37f66a7b3c59a8dd035
This commit is contained in:
Dean Lee
2023-08-25 03:55:47 +08:00
committed by GitHub
parent a1fe0701e9
commit 1a2f9c464b
4 changed files with 47 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
selfdrive/locationd/locationd.cc
View File

@@ -292,8 +292,8 @@ void Localizer::input_fake_gps_observations(double current_time) {
VectorXd current_x = this->kf->get_x();
VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
MatrixXdr ecef_pos_R = this->kf->get_fake_gps_pos_cov();
MatrixXdr ecef_vel_R = this->kf->get_fake_gps_vel_cov();
const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov();
const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov();
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
@@ -507,8 +507,8 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra
}
void Localizer::reset_kalman(double current_time) {
VectorXd init_x = this->kf->get_initial_x();
MatrixXdr init_P = this->kf->get_initial_P();
const VectorXd &init_x = this->kf->get_initial_x();
const MatrixXdr &init_P = this->kf->get_initial_P();
this->reset_kalman(current_time, init_x, init_P);
}
@@ -544,12 +544,12 @@ void Localizer::update_reset_tracker() {
}
}
void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos, VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R) {
void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) {
// too nonlinear to init on completely wrong
VectorXd current_x = this->kf->get_x();
MatrixXdr current_P = this->kf->get_P();
MatrixXdr init_P = this->kf->get_initial_P();
MatrixXdr reset_orientation_P = this->kf->get_reset_orientation_P();
const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P();
int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
@@ -565,7 +565,7 @@ void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd
this->reset_kalman(current_time, current_x, init_P);
}
void Localizer::reset_kalman(double current_time, VectorXd init_x, MatrixXdr init_P) {
void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) {
this->kf->init_state(init_x, init_P, current_time);
this->last_reset_time = current_time;
this->reset_tracker += 1.0;
@@ -620,7 +620,7 @@ bool Localizer::is_gps_ok() {
return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
}
bool Localizer::critical_services_valid(std::map<std::string, double> critical_services) {
bool Localizer::critical_services_valid(const std::map<std::string, double> &critical_services) {
for (auto &kv : critical_services){
if (kv.second >= INPUT_INVALID_THRESHOLD){
return false;
@@ -653,7 +653,7 @@ void Localizer::determine_gps_mode(double current_time) {
}
}
void Localizer::configure_gnss_source(LocalizerGnssSource source) {
void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
this->gnss_source = source;
if (source == LocalizerGnssSource::UBLOX) {
this->gps_std_factor = 10.0;

8
selfdrive/locationd/locationd.h
View File

@@ -33,13 +33,13 @@ public:
int locationd_thread();
void reset_kalman(double current_time = NAN);
void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos, Eigen::VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R);
void reset_kalman(double current_time, Eigen::VectorXd init_x, MatrixXdr init_P);
void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R);
void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P);
void finite_check(double current_time = NAN);
void time_check(double current_time = NAN);
void update_reset_tracker();
bool is_gps_ok();
bool critical_services_valid(std::map<std::string, double> critical_services);
bool critical_services_valid(const std::map<std::string, double> &critical_services);
bool is_timestamp_valid(double current_time);
void determine_gps_mode(double current_time);
bool are_inputs_ok();
@@ -95,5 +95,5 @@ private:
float gps_vertical_variance_factor;
double gps_time_offset;
void configure_gnss_source(LocalizerGnssSource source);
void configure_gnss_source(const LocalizerGnssSource &source);
};

40
selfdrive/locationd/models/live_kf.cc
View File

@@ -3,25 +3,25 @@
using namespace EKFS;
using namespace Eigen;
Eigen::Map<Eigen::VectorXd> get_mapvec(Eigen::VectorXd& vec) {
return Eigen::Map<Eigen::VectorXd>(vec.data(), vec.rows(), vec.cols());
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
}
Eigen::Map<MatrixXdr> get_mapmat(MatrixXdr& mat) {
return Eigen::Map<MatrixXdr>(mat.data(), mat.rows(), mat.cols());
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
}
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(std::vector<Eigen::VectorXd>& vec_vec) {
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
std::vector<Eigen::Map<Eigen::VectorXd>> res;
for (Eigen::VectorXd& vec : vec_vec) {
for (const Eigen::VectorXd &vec : vec_vec) {
res.push_back(get_mapvec(vec));
}
return res;
}
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(std::vector<MatrixXdr>& mat_vec) {
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
std::vector<Eigen::Map<MatrixXdr>> res;
for (MatrixXdr& mat : mat_vec) {
for (const MatrixXdr &mat : mat_vec) {
res.push_back(get_mapmat(mat));
}
return res;
@@ -43,20 +43,20 @@ LiveKalman::LiveKalman() {
// init filter
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
std::vector<int>{3}, std::vector<std::string>(), 0.8);
}
void LiveKalman::init_state(VectorXd& state, VectorXd& covs_diag, double filter_time) {
void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
MatrixXdr covs = covs_diag.asDiagonal();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
void LiveKalman::init_state(VectorXd& state, MatrixXdr& covs, double filter_time) {
void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
void LiveKalman::init_state(VectorXd& state, double filter_time) {
void LiveKalman::init_state(const VectorXd &state, double filter_time) {
MatrixXdr covs = this->filter->covs();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
@@ -81,7 +81,7 @@ std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
return R;
}
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, std::vector<VectorXd> meas, std::vector<MatrixXdr> R) {
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
std::optional<Estimate> r;
if (R.size() == 0) {
R = this->get_R(kind, meas.size());
@@ -94,29 +94,29 @@ void LiveKalman::predict(double t) {
this->filter->predict(t);
}
Eigen::VectorXd LiveKalman::get_initial_x() {
const Eigen::VectorXd &LiveKalman::get_initial_x() {
return this->initial_x;
}
MatrixXdr LiveKalman::get_initial_P() {
const MatrixXdr &LiveKalman::get_initial_P() {
return this->initial_P;
}
MatrixXdr LiveKalman::get_fake_gps_pos_cov() {
const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() {
return this->fake_gps_pos_cov;
}
MatrixXdr LiveKalman::get_fake_gps_vel_cov() {
const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() {
return this->fake_gps_vel_cov;
}
MatrixXdr LiveKalman::get_reset_orientation_P() {
const MatrixXdr &LiveKalman::get_reset_orientation_P() {
return this->reset_orientation_P;
}
MatrixXdr LiveKalman::H(VectorXd in) {
MatrixXdr LiveKalman::H(const VectorXd &in) {
assert(in.size() == 6);
Matrix<double, 3, 6, Eigen::RowMajor> res;
this->filter->get_extra_routine("H")(in.data(), res.data());
this->filter->get_extra_routine("H")((double*)in.data(), res.data());
return res;
}

28
selfdrive/locationd/models/live_kf.h
View File

@@ -16,37 +16,37 @@
using namespace EKFS;
Eigen::Map<Eigen::VectorXd> get_mapvec(Eigen::VectorXd& vec);
Eigen::Map<MatrixXdr> get_mapmat(MatrixXdr& mat);
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(std::vector<Eigen::VectorXd>& vec_vec);
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(std::vector<MatrixXdr>& mat_vec);
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
class LiveKalman {
public:
LiveKalman();
void init_state(Eigen::VectorXd& state, Eigen::VectorXd& covs_diag, double filter_time);
void init_state(Eigen::VectorXd& state, MatrixXdr& covs, double filter_time);
void init_state(Eigen::VectorXd& state, double filter_time);
void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
void init_state(const Eigen::VectorXd &state, double filter_time);
Eigen::VectorXd get_x();
MatrixXdr get_P();
double get_filter_time();
std::vector<MatrixXdr> get_R(int kind, int n);
std::optional<Estimate> predict_and_observe(double t, int kind, std::vector<Eigen::VectorXd> meas, std::vector<MatrixXdr> R = {});
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
void predict(double t);
Eigen::VectorXd get_initial_x();
MatrixXdr get_initial_P();
MatrixXdr get_fake_gps_pos_cov();
MatrixXdr get_fake_gps_vel_cov();
MatrixXdr get_reset_orientation_P();
const Eigen::VectorXd &get_initial_x();
const MatrixXdr &get_initial_P();
const MatrixXdr &get_fake_gps_pos_cov();
const MatrixXdr &get_fake_gps_vel_cov();
const MatrixXdr &get_reset_orientation_P();
MatrixXdr H(Eigen::VectorXd in);
MatrixXdr H(const Eigen::VectorXd &in);
private:
std::string name = "live";