From e4090cbef03211faa7a66b81f502836c264e58b7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Fri, 5 Jun 2020 17:05:20 +0200
Subject: [PATCH] Adapted to new AbsoluteFactor API, strange error on matrix
size
---
demos/mcapi_povcdl_estimation.cpp | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
index b8ee51e..c00c835 100644
--- a/demos/mcapi_povcdl_estimation.cpp
+++ b/demos/mcapi_povcdl_estimation.cpp
@@ -441,7 +441,7 @@ int main (int argc, char **argv) {
// Prior velocity factor
CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF1, "Vel0", t0);
FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.segment<3>(7), P_origin.block<3, 3>(6, 6));
- FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF1->getV(), nullptr, false);
+ FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF1->getV(), nullptr, false);
// Special trick to make things work in the IMU + IKin + FTPreint case
// 0. Call keyFrameCallback of processorIKin so that its kf_buffer contains the first KF0
@@ -467,14 +467,14 @@ int main (int argc, char **argv) {
Matrix6d Q_bi = 1e-8 * Matrix6d::Identity();
CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", Vector6d::Zero(), Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
// Add loose absolute factor on initial bp bias since dynamical trajectories
// able to excite the system enough to make COM and bias_p observable hard to generate in a simple gtest
Matrix3d Q_bp = pow(std_abs_biasp, 2)* Matrix3d::Identity();
CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1, "bp0", t0);
FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_prior, Q_bp);
- FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic(), nullptr, false);
+ FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic(), nullptr, false);
///////////////////////////////////////////
--
GitLab