From e798de7a618f8a2d5ac5196d35e3f6970e3073c8 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 9 Sep 2021 13:42:30 +0200
Subject: [PATCH] hotfix
---
include/core/factor/factor_velocity_direction_3d.h | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/include/core/factor/factor_velocity_direction_3d.h b/include/core/factor/factor_velocity_direction_3d.h
index 63ad133a9..ab8bd74db 100644
--- a/include/core/factor/factor_velocity_direction_3d.h
+++ b/include/core/factor/factor_velocity_direction_3d.h
@@ -36,7 +36,7 @@ class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d
_ftr_ptr->getFrame()->getO()),
min_vel_sq_norm_(_min_vel_norm*_min_vel_norm)
{
- assert(abs(_ftr_ptr->getMeasurement().norm() - 1.0) < wolf::EPS && "velocity direction measurement must be normalized");
+ assert(abs(_ftr_ptr->getMeasurement().norm() - 1.0) < wolf::Constants::EPS && "velocity direction measurement must be normalized");
// std::cout << "created FactorVelocityDirection3d " << std::endl;
}
@@ -74,7 +74,7 @@ inline bool FactorVelocityDirection3d::operator ()(const T* const _v, const T* c
// << v_local(2) << "\n";
// error (angle between measurement and velocity in local coordinates)
- T error = acos( v_local.dot(getMeasurement().cast<T>()) / (v_local.norm()));
+ T error = acos( v_local.dot(getMeasurement()) / (v_local.norm()));
// residual
_residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * error;
--
GitLab