Commit 50ab0b01 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Fixed bug on checking landmarks taking into account the predefined axis for a camera

parent 7ed798be
......@@ -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)
......
......@@ -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)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment