Resolve "Processor motion model"

Closes #407 (closed)

include/core/factor/factor_velocity_direction_3d.h

+
+#ifndef FACTOR_VELOCITY_DIRECTION_3D_H_
+#define FACTOR_VELOCITY_DIRECTION_3D_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+    
+WOLF_PTR_TYPEDEFS(FactorVelocityDirection3d);
+
+//class
+class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d,1,3,4>
+{
+    public:
+        FactorVelocityDirection3d(FeatureBasePtr _ftr_ptr,
+                                  const ProcessorBasePtr& _processor_ptr,
+                                  bool _apply_loss_function,
+                                  FactorStatus _status = FAC_ACTIVE) :
+                FactorAutodiff<FactorVelocityDirection3d,1,3,4>("FactorVelocityDirection3d",
+                                                                TOP_ABS,
+                                                                _ftr_ptr,
+                                                                nullptr, nullptr, nullptr, nullptr,
+                                                                _processor_ptr,
+                                                                _apply_loss_function,
+                                                                _status,
+                                                                _ftr_ptr->getFrame()->getV(),
+                                                                _ftr_ptr->getFrame()->getO())
+        {
+//            std::cout << "created FactorVelocityDirection3d " << std::endl;
+        }
+
+        ~FactorVelocityDirection3d() override = default;
+
+        template<typename T>
+        bool operator ()(const T* const _v, const T* const _o, T* _residuals) const;
+};
+
+template<typename T>
+inline bool FactorVelocityDirection3d::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);
+
+    //     std::cout << "v: " << v(0) << " "
+    //                        << v(1) << " "
+    //                        << v(2) << "\n";
+    //     std::cout << "q: " << q.coeffs()(0) << " "
+    //                        << q.coeffs()(1) << " "
+    //                        << q.coeffs()(2) << " "
+    //                        << q.coeffs()(3) << "\n";
+
+    // velocity in local coordinates
+    Eigen::Matrix<T,3,1> v_local = q.conjugate() * v;
+
+    //     std::cout << "v_local: " << v_local(0) << " "
+    //                              << v_local(1) << " "
+    //                              << v_local(2) << "\n";
+
+    // error (angle between measurement and velocity in local coordinates)
+    double error = acos( v_local.dot(getMeasurement().cast<T>()) / (v_local.norm() * getMeasurement().cast<T>().norm()));
+
+    // residual
+    _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * error;
+
+    return true;
+}
+
+} // namespace wolf
+
+#endif