diff --git a/src/adc_landmarks_slam_solver_alg.cpp b/src/adc_landmarks_slam_solver_alg.cpp
index 7d8067ec726d3b2aeb889ed09da76645127ac951..38b5cd8e98130e3fc99f358569b1ad815f1bd13c 100644
--- a/src/adc_landmarks_slam_solver_alg.cpp
+++ b/src/adc_landmarks_slam_solver_alg.cpp
@@ -173,7 +173,7 @@ double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector3d _fe
       global_land.pose.position.x = it->second.pose(0);
       global_land.pose.position.y = it->second.pose(1);
       tf2::Quaternion q;
-      q.setRPY(0.0, 0.0, it->second.pose(2));
+      q.setRPY(0.0, 0.0, it->second.pose(2));//TODO: Must take into acoount not just yaw, duality pitch
       global_land.pose.orientation = tf2::toMsg(q);
 
       tf2::doTransform(global_land, local_land, _tf_sensor_map);
@@ -182,11 +182,13 @@ double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector3d _fe
       q = tf2::Quaternion(local_land.pose.orientation.x, local_land.pose.orientation.y, local_land.pose.orientation.z, local_land.pose.orientation.w);
       tf2::Matrix3x3(q).getEulerYPR(land_yaw, pitch, roll);
 
-      landmark_pos << local_land.pose.position.x, local_land.pose.position.y;
+      // landmark_pos << local_land.pose.position.x, local_land.pose.position.y;
+      landmark_pos << local_land.pose.position.z, local_land.pose.position.x;//is a camera, axis are different
 
       double dist = mahalanobis_distance(feature_pos, _feature_r, _feature_th, landmark_pos);
       // std::cout << "check dist " << dist << std::endl;
-      if (dist <= this->config_.landmark_mahalanobis_dist && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(land_yaw -_feature_pose(2)) < this->config_.landmarks_orientation_th)))
+      //pitch instead of yaw beacuse is a camera
+      if (dist <= this->config_.landmark_mahalanobis_dist && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(pitch -_feature_pose(2)) < this->config_.landmarks_orientation_th)))
       {
         Eigen::Vector2d d = landmark_pos - feature_pos;
         if (d.norm() < min_dist)
diff --git a/src/adc_landmarks_slam_solver_alg_node.cpp b/src/adc_landmarks_slam_solver_alg_node.cpp
index a5fd3b37394c080c706e528131cc921fdb5e4159..bac784f71d84d7511294b4e6c1ec11ab48255014 100644
--- a/src/adc_landmarks_slam_solver_alg_node.cpp
+++ b/src/adc_landmarks_slam_solver_alg_node.cpp
@@ -648,7 +648,9 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
 
     q = tf2::Quaternion(local_feature.pose.orientation.x, local_feature.pose.orientation.y, local_feature.pose.orientation.z, local_feature.pose.orientation.w);
     tf2::Matrix3x3(q).getEulerYPR(yaw_local, pitch, roll);
-    feature_pose_local << local_feature.pose.position.x, local_feature.pose.position.y, yaw_local;
+    // feature_pose_local << local_feature.pose.position.x, local_feature.pose.position.y, yaw_local;
+    feature_pose_local << local_feature.pose.position.z, local_feature.pose.position.x, pitch;//is a camera. axis rotated
+
     (_front? this->front_features_: this->rear_features_)[i].landmark_key = this->alg_.is_a_mapped_landmark(feature_pose_local, (_front? this->front_features_: this->rear_features_)[i].r, (_front? this->front_features_: this->rear_features_)[i].th, (_front? this->front_features_: this->rear_features_)[i].type, (_front? this->front_features_frame_: this->rear_features_frame_), tf_sensor_map_msg);
 
     //if it isn't a mapped landmark, update them if enabled
@@ -726,10 +728,12 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, boo
       q = tf2::Quaternion(local_land.pose.orientation.x, local_land.pose.orientation.y, local_land.pose.orientation.z, local_land.pose.orientation.w);
       tf2::Matrix3x3(q).getEulerYPR(land_yaw, pitch, roll);
 
-      landmark_pos << local_land.pose.position.x, local_land.pose.position.y;
+      // landmark_pos << local_land.pose.position.x, local_land.pose.position.y;
+      landmark_pos << local_land.pose.position.z, local_land.pose.position.x;//is a camera, rotated axis
 
       double dist = this->alg_.mahalanobis_distance(feature_pos, _feature.r, _feature.th, landmark_pos);
-      if (dist <= this->config_.landmark_mahalanobis_dist && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(land_yaw -_feature_pose(2)) < this->config_.landmarks_orientation_th)))
+      // pitch instead of yaw beacuse is a camera
+      if (dist <= this->config_.landmark_mahalanobis_dist && ((!this->config_.landmarks_filter_orientation_en) || (std::fabs(pitch -_feature_pose(2)) < this->config_.landmarks_orientation_th)))
       {
         Eigen::Vector2d d = landmark_pos - feature_pos;
         if (d.norm() < min_dist)