spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Fix orientation in map (#21863) 

* fix calibrated orientation

* update ref

* cleaner

* update ref

* master

* update again

* update ref again again

* actually compiled this tim,e
pull/21865/head
HaraldSchafer 2021-08-05 21:06:46 -07:00 committed by GitHub
parent 276b00ccb8
commit e0087fc313
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
selfdrive/locationd/locationd.cc
View File

@ -82,8 +82,9 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
//fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr device_from_ecef = quat2rot(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
VectorXd calibrated_orientation_ecef = rot2euler(this->calib_from_device * device_from_ecef);
MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
//VectorXd calibrated_orientation_ecef = rot2euler(device_from_ecef);
VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
@ -115,6 +116,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
//orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
VectorXd nextfix_ecef = fix_ecef + vel_ecef;
VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
//ned_vel_std = self.converter->ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter->ecef2ned(fix_ecef + vel_ecef)
@ -137,6 +139,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, true);
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated);
init_measurement(fix.initOrientationNED(), orientation_ned, nans, true);
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, true);
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
1d005ab2b4f6747ec0cae5d1a5e251131df453e8
8a2cac15ef8decb0881eb7e67e47de8a81e592aa

2
selfdrive/ui/qt/maps/map.cc
View File

@ -129,7 +129,7 @@ void MapWindow::timerUpdate() {
if (localizer_valid) {
auto pos = location.getPositionGeodetic();
auto orientation = location.getOrientationNED();
auto orientation = location.getCalibratedOrientationNED();
float velocity = location.getVelocityCalibrated().getValue()[0];
float bearing = RAD2DEG(orientation.getValue()[2]);

2
selfdrive/ui/qt/maps/map_helpers.cc
View File

@ -17,7 +17,7 @@ QMapbox::CoordinatesCollections model_to_collection(
Eigen::Vector3d ecef(positionECEF.getValue()[0], positionECEF.getValue()[1], positionECEF.getValue()[2]);
Eigen::Vector3d orient(calibratedOrientationECEF.getValue()[0], calibratedOrientationECEF.getValue()[1], calibratedOrientationECEF.getValue()[2]);
Eigen::Matrix3d ecef_from_local = euler2rot(orient).transpose();
Eigen::Matrix3d ecef_from_local = euler2rot(orient);
QMapbox::Coordinates coordinates;
auto x = line.getX();