mmo-ref/og-libsurvive
1
0
Fork 0
mirror of https://github.com/collabora/libsurvive.git synced 2026-08-04 00:51:21 -04:00
Code Issues Projects Releases Packages Wiki Activity Actions

Fixed bad diag size

Browse source 
Fixed bad diff call in test
This commit is contained in:
Justin Berger 2022-02-13 12:32:47 -07:00
parent fd8faaff30
commit affdef0dbd
2 changed files with 6 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/survive_kalman_lighthouses.c
View file

@ -92,7 +92,7 @@ void survive_kalman_lighthouse_update_position(SurviveKalmanLighthouse *tracker,
cnCopy(&tracker->model.P, &tracker->push_cov, 0);
SurvivePose min_error = {.Pos = {1e-5, 1e-5, 1e-5}, .Rot = {1e-6, 1e-6, 1e-6, 1e-6}};
CnMat minError = cnVec(7, min_error.Pos);
CnMat minError = cnVec(6, min_error.Pos);
cn_add_diag(&tracker->push_cov, &minError, 5);
tracker->state.Lighthouse = ctx->bsd[tracker->lh].Pose;

6
src/test_cases/test_replays.c
View file

@ -246,9 +246,13 @@ static int test_path(const char *filename, int main_argc, char **main_argv) {
FLT err[2] = {0};
ApplyPoseToPose(&pose, &original2current, &pose);
if (!quatiszero(pose.Rot))
if (!quatiszero(pose.Rot) && !quatiszero(ctx->bsd[i].Pose.Rot))
diff(err, &pose, &ctx->bsd[i].Pose);
if(!quatiszero(pose.Rot) && ctx->bsd[i].PositionSet == 0) {
err[0] = INFINITY;
}
fprintf(stderr, " " SurvivePose_format "\terr: %f %f\n", pose.Pos[0], pose.Pos[1], pose.Pos[2],
pose.Rot[0], pose.Rot[1], pose.Rot[2], pose.Rot[3], err[0], err[1]);