Resolve "Processor motion model"

Closes #407 (closed)

include/core/factor/factor_velocity_local_direction_3d.h

+
+#ifndef FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
+#define FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+namespace wolf {
+    
+WOLF_PTR_TYPEDEFS(FactorVelocityLocalDirection3d);
+
+//class
+class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalDirection3d,2,3,4>
+{
+    protected:
+        double min_vel_sq_norm_; // stored in squared norm for efficiency
+        int orthogonal_axis_; // 0: X, 1: Y, 2: Z
+
+    public:
+        FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
+                                       const double& _min_vel_norm,
+                                       const ProcessorBasePtr& _processor_ptr,
+                                       bool _apply_loss_function,
+                                       FactorStatus _status = FAC_ACTIVE);
+
+        ~FactorVelocityLocalDirection3d() override = default;
+
+        template<typename T>
+        bool operator ()(const T* const _v, const T* const _o, T* _residuals) const;
+
+        template<typename T>
+        Eigen::Matrix<T,2,1> computeElevationAzimuth(const Eigen::Matrix<T,3,1> vector) const
+        {
+            Eigen::Matrix<T,2,1> elev_azim;
+
+            // plane YZ
+            if (orthogonal_axis_ == 0)
+            {
+                elev_azim(0) = asin(vector(0) / vector.norm());
+                elev_azim(1) = atan2(vector(2), vector(1));
+            }
+            // plane XZ
+            if (orthogonal_axis_ == 1)
+            {
+                elev_azim(0) = asin(vector(1) / vector.norm());
+                elev_azim(1) = atan2(vector(0), vector(2));
+            }
+            // plane XY
+            if (orthogonal_axis_ == 2)
+            {
+                elev_azim(0) = asin(vector(2) / vector.norm());
+                elev_azim(1) = atan2(vector(1), vector(0));
+            }
+
+            return elev_azim;
+        }
+};
+
+FactorVelocityLocalDirection3d::FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
+                                                               const double& _min_vel_norm,
+                                                               const ProcessorBasePtr& _processor_ptr,
+                                                               bool _apply_loss_function,
+                                                               FactorStatus _status) :
+        FactorAutodiff<FactorVelocityLocalDirection3d,2,3,4>("FactorVelocityLocalDirection3d",
+                                                             TOP_ABS,
+                                                             _ftr_ptr,
+                                                             nullptr, nullptr, nullptr, nullptr,
+                                                             _processor_ptr,
+                                                             _apply_loss_function,
+                                                             _status,
+                                                             _ftr_ptr->getFrame()->getV(),
+                                                             _ftr_ptr->getFrame()->getO()),
+        min_vel_sq_norm_(_min_vel_norm*_min_vel_norm)
+{
+    MatrixSizeCheck<3,1>::check(_ftr_ptr->getMeasurement());
+    MatrixSizeCheck<1,1>::check(_ftr_ptr->getMeasurementSquareRootInformationUpper());
+
+    /* Decide the plane (two axis A1, A2 from X, Y, Z in local coordinates) in which compute elevation and azimuth
+     *    - elevation w.r.t. the plane
+     *    - azimuth computed with atan2(A2, A1)
+     *
+     * This is done to avoid the degenerated case: elevation = +/-PI/2
+     * We select the orthogonal axis as the farthest to the desired velocity in local coordinates,
+     * so the component one with lower value.
+     */
+
+    measurement_.minCoeff(&orthogonal_axis_);
+
+    // measurement: elevation & azimuth (w.r.t. selected plane)
+    measurement_ = computeElevationAzimuth(Eigen::Vector3d(measurement_));
+    measurement_sqrt_information_upper_ = Matrix2d::Identity() * measurement_sqrt_information_upper_(0,0);
+}
+
+template<typename T>
+inline bool FactorVelocityLocalDirection3d::operator ()(const T* const _v, const T* const _q, T* _residuals) const
+{
+    Eigen::Map<const Eigen::Matrix<T,3,1> > v(_v);
+    Eigen::Map<const Eigen::Quaternion<T> > q(_q);
+    Eigen::Map<Eigen::Matrix<T,2,1> > residuals(_residuals);
+
+    if (v.squaredNorm() < min_vel_sq_norm_)
+    {
+        _residuals[0] = T(0);
+        return true;
+    }
+
+//    std::cout << "----------\n";
+//    std::cout << "desired elevation: " << getMeasurement()(0) << "\n";
+//    std::cout << "desired azimuth:   " << getMeasurement()(1) << "\n";
+
+//    std::cout << "v: \n\t" << v(0) << "\n\t"
+//                           << v(1) << "\n\t"
+//                           << v(2) << "\n";
+//
+//    std::cout << "q: \n\t" << q.coeffs()(0) << "\n\t"
+//                           << q.coeffs()(1) << "\n\t"
+//                           << q.coeffs()(2) << "\n\t"
+//                           << q.coeffs()(3) << "\n";
+
+    Eigen::Matrix<T,3,1> v_local = q.conjugate() * v;
+//    std::cout << "v (local): \n\t" << v_local(0) << "\n\t"
+//                                   << v_local(1) << "\n\t"
+//                                   << v_local(2) << "\n";
+
+    // expectation
+    Eigen::Matrix<T,2,1> exp_elev_azim = computeElevationAzimuth(v_local);
+//    std::cout << "expected elevation: " << exp_elev_azim(0) << "\n";
+//    std::cout << "expected azimuth:   " << exp_elev_azim(1) << "\n";
+
+    // error
+    Eigen::Matrix<T,2,1> error = getMeasurement() - exp_elev_azim;
+    pi2pi(error(1));
+//    std::cout << "error (corrected): \n\t" << error(0) << "\n\t"
+//                                           << error(1) << "\n;
+
+    // residuals
+    residuals = getMeasurementSquareRootInformationUpper() * error;
+//    std::cout << "residuals: \n\t" << residuals(0) << "\n\t"
+//                                   << residuals(1) << "\n;
+
+    return true;
+}
+
+} // namespace wolf
+
+#endif