From 60fd7d8b40f7644efbff4a5824352d6b8b41cf0b Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 9 Sep 2021 13:30:44 +0200
Subject: [PATCH] efficiency issues
---
include/core/factor/factor_velocity_direction_3d.h | 9 +++++----
1 file changed, 5 insertions(+), 4 deletions(-)
diff --git a/include/core/factor/factor_velocity_direction_3d.h b/include/core/factor/factor_velocity_direction_3d.h
index 0704416d7..63ad133a9 100644
--- a/include/core/factor/factor_velocity_direction_3d.h
+++ b/include/core/factor/factor_velocity_direction_3d.h
@@ -17,7 +17,7 @@ WOLF_PTR_TYPEDEFS(FactorVelocityDirection3d);
class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d,1,3,4>
{
protected:
- double min_vel_norm_;
+ double min_vel_sq_norm_; // stored in squared norm for efficiency
public:
FactorVelocityDirection3d(FeatureBasePtr _ftr_ptr,
@@ -34,8 +34,9 @@ class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d
_status,
_ftr_ptr->getFrame()->getV(),
_ftr_ptr->getFrame()->getO()),
- min_vel_norm_(_min_vel_norm)
+ 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");
// std::cout << "created FactorVelocityDirection3d " << std::endl;
}
@@ -51,7 +52,7 @@ inline bool FactorVelocityDirection3d::operator ()(const T* const _v, const T* c
Eigen::Map<const Eigen::Matrix<T,3,1> > v(_v);
Eigen::Map<const Eigen::Quaternion<T> > q(_q);
- if (v.norm() < min_vel_norm_)
+ if (v.squaredNorm() < min_vel_sq_norm_)
{
_residuals[0] = T(0);
return true;
@@ -73,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() * getMeasurement().cast<T>().norm()));
+ T error = acos( v_local.dot(getMeasurement().cast<T>()) / (v_local.norm()));
// residual
_residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * error;
--
GitLab