From 5e7679f06c398cc6adc4c171fa7b29ba05b2dd3d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sun, 12 Nov 2017 23:25:16 +0100
Subject: [PATCH] Fix covariance size
---
src/problem.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/problem.cpp b/src/problem.cpp
index 513cc64c2..8c639c114 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -613,7 +613,7 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen:
// Capture fix only takes 3D position and Quaternion orientation
CaptureFixPtr init_capture;
if (trajectory_ptr_->getFrameStructure() == "POV 3D")
- init_capture = std::make_shared<CaptureFix>(_ts, nullptr, _prior_state.head(7), _prior_cov);
+ init_capture = std::make_shared<CaptureFix>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
else
init_capture = std::make_shared<CaptureFix>(_ts, nullptr, _prior_state, _prior_cov);
--
GitLab