sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
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: 3930ec9fac
beeps
Dean Lee 2 years ago committed by GitHub
parent a1fe0701e9
commit 1a2f9c464b
4 changed files with 47 additions and 47 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      selfdrive/locationd/locationd.cc
  2. 8
      selfdrive/locationd/locationd.h
  3. 40
      selfdrive/locationd/models/live_kf.cc
  4. 28
      selfdrive/locationd/models/live_kf.h

18
selfdrive/locationd/locationd.cc
Unescape 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
Unescape 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
Unescape 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
Unescape 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";

Write Preview
Loading…
Cancel
Save