[WORK IN PROGRESS] Added landmark_residual_froms_base. Some minor changes

include/adc_landmarks_slam_solver_alg.h

@@ -46,6 +46,7 @@
 typedef struct {
   double r; ///< Range at detection
   double theta; ///< Angle at detection
+  double landmark_key; ///< Detected landmark's ID.
 }LandmarkResidualData;
 
 /**

include/landmark_residual_2D.h

@@ -76,8 +76,8 @@ class LandmarkResidual2D {
       return true;
     }
   private:
-    double r_; ///< The measured distance to the reflector center.
-    double theta_; ///< The measured angle to the reflector.
+    double r_; ///< The measured distance to the landmark center.
+    double theta_; ///< The measured angle to the landmark.
     double sensor_sigma_r_; ///< Sensor radial sigma.
     double sensor_sigma_th_; ///< Sensor angular sigma.
     bool sensor_inverted_; ///<If the sensor is inverted

include/landmark_residual_from_base_2D.h

+#include "ceres/ceres.h"
+#include <math.h>
+#include <Eigen/Eigen>
+
+#define MAX_ROLL 0.2
+/**
+ * \brief Class to define the residuals between a robot pose and a landmark detected.
+ *
+ */
+class LandmarkResidualFromBase2D {
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    /**
+     * \brief The constructor.
+     *
+     * \param _r Detection range from base link.
+     *
+     * \param _theta Detection angle from base link.
+     *
+     * \param _sigma_r Sensor radial standard deviation.
+     *
+     * \param _sigma_th Sensor angular standard deviation.
+     *
+     * \param _sensor_roll Sensor roll to know if it is inverted.
+     *
+     */
+    LandmarkResidualFromBase2D(const double& _r, const double& _theta, const double& _sigma_r, const double& _sigma_th, const double& _sensor_roll)
+    {
+      r_ = _r;
+      theta_ = _theta;
+      sensor_sigma_r_ = _sigma_r;
+      sensor_sigma_th_ = _sigma_th;
+      sensor_inverted_ = (std::fabs(_sensor_roll) > MAX_ROLL);
+    }
+
+    template <typename T>
+    /**
+    * \brief Function to calculate the error for Ceres optimization.
+    *
+    * \return True if all is ok.
+    */
+    bool operator()(const T* const _robot_pose,
+                    const T* const _landmark_pos,
+                   T* residual) const {
+    
+      Eigen::Matrix<T,2,1> dist;
+      Eigen::Matrix<T,3,1> landmark_global, landmark_local;
+      Eigen::Matrix<T,3,1> trans;
+      Eigen::Matrix<T,3,3> rot;
+
+
+      landmark_global << _landmark_pos[0], _landmark_pos[1], T(0.0);
+      trans << _robot_pose[0], _robot_pose[1], _robot_pose[2];
+      rot << cos(_robot_pose[2]), sin(_robot_pose[2]), T(0.0),
+             -sin(_robot_pose[2]), cos(_robot_pose[2]), T(0.0),
+             T(0.0), T(0.0), T(1.0);
+      landmark_local = rot*(landmark_global - trans);
+
+      dist << landmark_local(0), landmark_local(1)*(sensor_inverted_? T(-1): T(1)); 
+
+      T angle_res = T(theta_) - T(atan2(dist(1), dist(0)));
+      while (angle_res >= M_PI)
+        angle_res -= 2*M_PI;
+      while (angle_res < -M_PI)
+        angle_res += 2*M_PI;
+
+      residual[0] = (T(r_) - dist.norm())/T(sensor_sigma_r_);
+      residual[1] = angle_res/T(sensor_sigma_th_);
+      return true;
+    }
+  private:
+    double r_; ///< The measured distance to the landmark center from base link.
+    double theta_; ///< The measured angle to the landmark from base link.
+    double sensor_sigma_r_; ///< Sensor radial sigma.
+    double sensor_sigma_th_; ///< Sensor angular sigma.
+    bool sensor_inverted_; ///<If the sensor is inverted
+};
\ No newline at end of file