Change math.h --> cmath. Do some casts to wolf::Scalar to be able to accept float.

This has been a pain. Compatibility with wolf::Scalar float is now broken.
This commit fixed a few issues, but there are too many.
One day, maybe, we'll want to use float.
By now, everything goes well with wolf::Scalar double.

src/capture_odom_2D.cpp

 #include "capture_odom_2D.h"
+#include <cmath>
 
 namespace wolf {
 
@@ -7,9 +8,9 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts, const TimeStamp& _final_
         CaptureMotion(_init_ts, _final_ts, _sensor_ptr, _data)
 {
     data_covariance_ = Eigen::Matrix3s::Zero();
-    data_covariance_(0, 0) = std::max(1e-10, _data(0) * ((SensorOdom2D*)_sensor_ptr)->getDispVarToDispNoiseFactor());
-    data_covariance_(1, 1) = std::max(1e-10, _data(1) * ((SensorOdom2D*)_sensor_ptr)->getDispVarToDispNoiseFactor());
-    data_covariance_(2, 2) = std::max(1e-10, _data(2) * ((SensorOdom2D*)_sensor_ptr)->getRotVarToRotNoiseFactor());
+    data_covariance_(0, 0) = std::max((Scalar)1e-10, _data(0) * ((SensorOdom2D*)_sensor_ptr)->getDispVarToDispNoiseFactor());
+    data_covariance_(1, 1) = std::max((Scalar)1e-10, _data(1) * ((SensorOdom2D*)_sensor_ptr)->getDispVarToDispNoiseFactor());
+    data_covariance_(2, 2) = std::max((Scalar)1e-10, _data(2) * ((SensorOdom2D*)_sensor_ptr)->getRotVarToRotNoiseFactor());
 //  std::cout << data_covariance_ << std::endl;
 }
 

src/ceres_wrapper/cost_function_wrapper.h

@@ -39,10 +39,10 @@ class CostFunctionWrapper : public ceres::CostFunction
             // load parameters evaluation value
             std::vector<Eigen::Map<const Eigen::VectorXs>> state_blocks_map_;
             for (unsigned int i = 0; i < state_blocks_sizes_.size(); i++)
-                state_blocks_map_.push_back(Eigen::Map<const Eigen::VectorXs>(parameters[i], state_blocks_sizes_[i]));
+                state_blocks_map_.push_back(Eigen::Map<const Eigen::VectorXs>((Scalar*)parameters[i], state_blocks_sizes_[i]));
 
             // residuals
-            Eigen::Map<Eigen::VectorXs> residuals_map(residuals, constraint_ptr_->getSize());
+            Eigen::Map<Eigen::VectorXs> residuals_map((Scalar*)residuals, constraint_ptr_->getSize());
             residuals_map = constraint_ptr_->evaluateResiduals(state_blocks_map_);
 
             // also compute jacobians
@@ -55,7 +55,7 @@ class CostFunctionWrapper : public ceres::CostFunction
                 {
                     compute_jacobians_[i] = (jacobians[i] != nullptr);
                     if (jacobians[i] != nullptr)
-                        jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>(jacobians[i], constraint_ptr_->getSize(), state_blocks_sizes_[i]));
+                        jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>((Scalar*)jacobians[i], constraint_ptr_->getSize(), state_blocks_sizes_[i]));
                     else
                         jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>(nullptr, 0, 0)); //TODO: check if it can be done
                 }

src/ceres_wrapper/local_parametrization_wrapper.cpp

@@ -4,16 +4,16 @@ namespace wolf {
 
 bool LocalParametrizationWrapper::Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const
 {
-    Eigen::Map<const Eigen::VectorXs> x_raw_map(x_raw, GlobalSize());
-    Eigen::Map<const Eigen::VectorXs> delta_raw_map(delta_raw, LocalSize());
-    Eigen::Map<Eigen::VectorXs> x_plus_map(x_plus_delta_raw, GlobalSize());
+    Eigen::Map<const Eigen::VectorXs> x_raw_map((Scalar*)x_raw, GlobalSize());
+    Eigen::Map<const Eigen::VectorXs> delta_raw_map((Scalar*)delta_raw, LocalSize());
+    Eigen::Map<Eigen::VectorXs> x_plus_map((Scalar*)x_plus_delta_raw, GlobalSize());
     return local_parametrization_ptr_->plus(x_raw_map, delta_raw_map, x_plus_map);
 };
 
 bool LocalParametrizationWrapper::ComputeJacobian(const double* x, double* jacobian) const
 {
-    Eigen::Map<const Eigen::VectorXs> x_map(x, GlobalSize());
-    Eigen::Map<Eigen::MatrixXs> jacobian_map(jacobian, GlobalSize(), LocalSize());
+    Eigen::Map<const Eigen::VectorXs> x_map((Scalar*)x, GlobalSize());
+    Eigen::Map<Eigen::MatrixXs> jacobian_map((Scalar*)jacobian, GlobalSize(), LocalSize());
     return local_parametrization_ptr_->computeJacobian(x_map, jacobian_map);
 };
 

src/examples/test_opencv.cpp

@@ -17,7 +17,8 @@
 #include "opencv2/imgproc/imgproc.hpp"
 
 // General includes
-#include <math.h>
+//#include <math.h>
+#include <cmath>
 #include <complex>      // std::complex, std::norm
 
 //std includes

src/processor_brisk.h

@@ -16,7 +16,7 @@
 #include <opencv2/core/core.hpp>
 
 // General includes
-#include <math.h>
+#include <cmath>
 #include <complex>      // std::complex, std::norm
 
 

src/sensor_camera.cpp

@@ -2,7 +2,6 @@
 #include "state_block.h"
 #include "state_quaternion.h"
 
-
 namespace wolf {
 
 /**
@@ -42,4 +41,5 @@ SensorBase* SensorCamera::create(const std::string& _unique_name, const Eigen::V
     return sen_ptr;
 }
 
+
 } // namespace wolf

src/sensor_gps_fix.h

@@ -42,7 +42,7 @@ class SensorGPSFix : public SensorBase
          * Returns noise standard deviation
          * 
          **/        
-        double getNoise() const;
+        Scalar getNoise() const;
         
     public:
         static SensorBase* create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBase* _intrinsics);

src/sensor_odom_2D.h

@@ -44,14 +44,14 @@ class SensorOdom2D : public SensorBase
          * Returns displacement noise factor
          * 
          **/        
-        double getDispVarToDispNoiseFactor() const;
+        Scalar getDispVarToDispNoiseFactor() const;
 
         /** \brief Returns rotation noise factor
          * 
          * Returns rotation noise factor
          * 
          **/        
-        double getRotVarToRotNoiseFactor() const;
+        Scalar getRotVarToRotNoiseFactor() const;
         
 
 	public: