From c589d91096f6d34c665e67995061a7d068f99c43 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Fri, 23 Sep 2022 17:05:17 +0200
Subject: [PATCH] comment out IMU hack
---
src/node.cpp | 10 ++++++++++
1 file changed, 10 insertions(+)
diff --git a/src/node.cpp b/src/node.cpp
index 5f2257c..4901d6f 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -58,6 +58,16 @@ WolfRosNode::WolfRosNode()
ROS_INFO("Creating problem...");
problem_ptr_ = Problem::autoSetup(server);
+ // // HACK -> SENSOR PRIORS
+ // auto imu = problem_ptr_->findSensor("IMU");
+ // double std_acc = server.getParam<double>("sensor/IMU/ab_initial_stdev");
+ // double std_gyro = server.getParam<double>("sensor/IMU/wb_initial_stdev");
+ // Array<double,6,1> std_bias;
+ // std_bias << std_acc, std_acc, std_acc, std_gyro, std_gyro, std_gyro;
+ // imu->addPriorParameter('I', // bias
+ // Vector6d::Zero(), // mean
+ // std_bias.square().matrix().asDiagonal()); // cov
+
// SOLVER
ROS_INFO("Creating solver...");
solver_ = FactorySolver::create("SolverCeres", problem_ptr_, server);
--
GitLab