Localization with flipped RGBD

Author: rishabh1b

src/rbiyani_project_5/adventure_simulator/adventure_description/finn/finn.urdf.xacro

@@ -8,7 +8,7 @@
   <!-- Sensors... FIXME -->
 
   <xacro:include filename="$(find adventure_description)/urdf/sensors/asus_xtion_pro.urdf.xacro"/>
-  <xacro:sensor_asus_xtion_pro parent="base_link" xyz="0.14 0.02 0.1" rpy="0 0 0"/>
+  <xacro:sensor_asus_xtion_pro parent="base_link" xyz="0.14 -0.02 0.1" rpy="3.14 0 0"/>
     <!-- origin xyz="${0} 0 0" rpy="0.0 0.0 0.0"/-->
   <!-- /xacro:sensor_asus_xtion_pro -->
 

src/rbiyani_project_5/adventure_slam/src/Localizer.cpp

@@ -1,6 +1,7 @@
 #include "adventure_slam/Localizer.h"
 #include "adventure_slam/Line.h"
 #include <cmath>
+#include "ros/ros.h"
 
 #define PI 3.14159265
 
@@ -26,7 +27,7 @@ void Localizer::estimate()
   if (sz != 0)
      del_yaw = del_yaw / sz;
 
-  if (std::abs(del_yaw) > 0.01)
+  if (std::abs(del_yaw) > 0.1)
       delta_yaw += del_yaw;
 
   if (delta_yaw >= 360)
@@ -42,13 +43,15 @@ void Localizer::estimate()
   double curr_shift_x = 0, curr_shift_y = 0;
   int i, j, count = 0;
   double first_distance, first_angle, second_distance, sec_angle;
-  
+  ROS_INFO("Angle: %d", sz);
   for (i = 0; i < sz ; i++)
   {
+    //Simple Averaging approach
     LineMatcher::Pair curr_pair = matched_pairs[i];
     first_distance = curr_pair.L1.distance - curr_pair.L2.distance;
     first_angle = (curr_pair.L1.angle + curr_pair.L2.angle) / 2;
-    if ((abs(first_angle) > 0 && abs(first_angle) < 10) || (abs(first_angle) > 80 && abs(first_angle) < 100) || (abs(first_angle) > 170 && abs(first_angle) < 190)) 
+    ROS_INFO("Angle: %f", first_angle);
+    if ((abs(first_angle) > 0 && abs(first_angle) < 40) || (abs(first_angle) > 80 && abs(first_angle) < 120) || (abs(first_angle) > 160 && abs(first_angle) < 200)) 
     {
       first_angle = first_angle * PI / 180;
 
@@ -58,8 +61,8 @@ void Localizer::estimate()
     }
   }
 
-    /* Following logic used Line Intersection Logic
-    if (sz == 1)
+    // Following logic used Line Intersection Logic
+   /*if (sz == 1)
     {
        curr_shift_x += first_distance * std::cos(first_angle);
        curr_shift_y += first_distance * std::sin(first_angle);
@@ -85,9 +88,9 @@ void Localizer::estimate()
     curr_shift_y = curr_shift_y / count;
   }
 
-  //if(std::abs(curr_shift_x) > 0.01)
+  if(std::abs(curr_shift_x) > 0.001)
     shift_x += curr_shift_x;
-  //if(std::abs(curr_shift_y) > 0.01)
+  if(std::abs(curr_shift_y) > 0.001)
     shift_y += curr_shift_y; 
 
 }

src/rbiyani_project_5/adventure_slam/src/adventure_slam.cpp

@@ -45,10 +45,10 @@ LaserScanProcessor::LaserScanProcessor(ros::NodeHandle n_)
   this->vo_pub = n.advertise<nav_msgs::Odometry>("/vo",1);
 
   //Initialize the odom_visual to odom Broadcaster
-  tf::Transform transform;
-  transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
-  transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
-  br_vo_o.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom_visual", "odom"));
+  //tf::Transform transform;
+  //transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
+  //transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
+  //br_vo_o.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom_visual", "odom"));
 }
 
 void LaserScanProcessor::laser_callback(const sensor_msgs::LaserScan& scan)
@@ -66,6 +66,10 @@ void LaserScanProcessor::laser_callback(const sensor_msgs::LaserScan& scan)
     float angle = scan.angle_min;
     std::vector<pcl::PointXYZ> points;
     std::vector<float> ranges = scan.ranges;
+
+    // FIX: Make the ranges go reverse to support reverse camera placement
+    std::reverse(ranges.begin(),ranges.end());
+
     float theta, r; 
     tf::Vector3 v, v_glob, v_glob_vo;
 
@@ -81,7 +85,7 @@ void LaserScanProcessor::laser_callback(const sensor_msgs::LaserScan& scan)
             continue;
         if (isnan(r))
             continue;
-        point.x = (r * std::sin(theta)); //camera_depth_optical_frame has actually Z direction pointing forwards. 
+        point.x = (-r * std::sin(theta)); //camera_depth_optical_frame has actually Z direction pointing forwards. 
         point.z = (r * std::cos(theta)); //Initially, We rather took y direction pointing forwards. i.e. swap z and y and get all
         point.y = 0;                     //co-ordinates in first two-components of the PCLXYZ object and work with 
                                          //all are equations in X-Y Plane(rather than X-Z). It is better to work in X-Z so that
@@ -150,8 +154,6 @@ void LaserScanProcessor::laser_callback(const sensor_msgs::LaserScan& scan)
     //v_glob.setY(loc.shift_x);
     //v_glob.setZ(0);
 
-    //TODO: Need to transform v_glob to base_footprint - static transform-will be only translation
-    // For now robot's pose point is taken to be as the point on the optical depth sensor
     v_glob_vo = (this->vo_fixed_to_base) * v_glob;
 
     //Correct for the difference in origin
@@ -177,7 +179,7 @@ void LaserScanProcessor::laser_callback(const sensor_msgs::LaserScan& scan)
     result_pose.pose.pose.position.z = 0;
 
      //tf::Quaternion res = vo_fixed_to_base.getRotation() * tf::createQuaternionFromRPY(0,loc.delta_yaw,0);
-     tf::Quaternion res = tf::createQuaternionFromRPY(0, 0, -loc.delta_yaw * M_PI / 180);
+     tf::Quaternion res = tf::createQuaternionFromRPY(0, 0, loc.delta_yaw * M_PI / 180);
 
     //Publishing the other way round
     //tf::Quaternion res = tf::createQuaternionFromRPY(0, 0, -loc.delta_yaw);
@@ -230,11 +232,11 @@ void LaserScanProcessor::laser_callback(const sensor_msgs::LaserScan& scan)
 
     tf::Transform odom_to_ov;
     odom_to_ov = base_to_ov * base_to_odom.inverse();
-    br_vo_o.sendTransform(tf::StampedTransform(odom_to_ov, ros::Time::now(), "/odom_visual", "/odom"));
-    br_vo_bf.sendTransform(tf::StampedTransform(base_to_ov, ros::Time::now(), "/odom_visual", "/base_footprint"));
+    //br_vo_o.sendTransform(tf::StampedTransform(odom_to_ov, ros::Time::now(), "/odom_visual", "/odom"));
+    //br_vo_bf.sendTransform(tf::StampedTransform(base_to_ov, ros::Time::now(), "/odom_visual", "/base_footprint"));
 
     //Attach base_footprint to odom_visual directly
-    //br_vo_bf.sendTransform(tf::StampedTransform(base_to_ov.inverse(), ros::Time::now(), "/base_footprint", "/odom_visual"));
+    br_vo_bf.sendTransform(tf::StampedTransform(base_to_ov.inverse(), ros::Time::now(), "/base_footprint", "/odom_visual"));
 
 }