Camera class added and fiducial marker detections

rviz_simulator/CMakeLists.txt

@@ -262,4 +262,4 @@ target_link_libraries(simulate
 #   # myfile1
 #   # myfile2
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
+# )

rviz_simulator/README.md

@@ -6,7 +6,7 @@ The `rviz_simulator` is an interactive 3D environment built using RViz and Inter
 
 
 ## Installation Instructions
-Requires ROS Kinectic Kane.  
+Requires ROS Kinectic Kame.  
 
 Clone the repo into the `src` folder of your catkin workspace.
 Run `catkin_make` in the root of your workspace to build the `rviz_simulator` package.  
@@ -35,10 +35,34 @@ and 4 nodes in `debug_mode`:
 To enter `debug_mode`, run the launch file with the command line arg 
 > `debug_mode:=true`
 
-By default, the `photoneo_camera_intrinsics.yaml` file is loaded. To specify another camera intrinsics file, run the launch file with the command line arg
+By default, the `rviz_simulator/config/photoneo_camera_intrinsics.yaml` file is loaded. To specify another camera intrinsics file, run the launch file with the command line arg
 > `camera_intrinsics_file:=<camera_intrinsics_file_name.yaml>`  
 
-The `simulator` node creates a new folder `"detections_ROS_timestamp"` in the `rviz_simulator` package folder. A new detections folder with a new timestamp is created each time `roslaunch` is run.
+The `simulator` node creates a new folder with the name `"detections_<ROS_timestamp>"` in the `rviz_simulator` package folder.  
+E.g. `detections_1565880269`:
+```
+world_T_camera:
+  rotation: [3.1404926443667587, -6.1629514791161225e-08, -0.083128525595253286]
+  translation: [-3.4547343254089355, 1.3038516098695761e-07, 2.4355044364929199]
+detections:
+  - targetID: 3
+    size: [0.10000000000000001, 0.10000000000000001]
+    corners:
+      0: [749.04850065520918, 426.08777643692048]
+      1: [795.65300431471246, 426.13839052125752]
+      2: [795.65300129230377, 380.00886350617435]
+      3: [749.04849785406611, 380.05948125315223]
+  - targetID: 4
+    size: [0.10000000000000001, 0.10000000000000001]
+    corners:
+      0: [294.01203861603608, 425.59359128189027]
+      1: [338.63436644541946, 425.64205265550714]
+      2: [338.63436555204686, 380.50523728899429]
+      3: [294.0120379256731, 380.55370216948273]
+```
+
+
+A new detections folder with a new timestamp is created each time `roslaunch` is run.
 
 Drag around the virtual camera and multiple virtual fiducial targets.  
 

rviz_simulator/config/initialize_simulator.yaml

@@ -2,6 +2,7 @@ world_frame_id: "ROSWorld"
 
 # target parameters
 num_targets: 5
+target_size: [.1, 0.1, 0.01]
 distance_between_targets: 1.0
 starting_target_position: [0.0, 0.0, 0.0]                   # geometery_msgs::Point
 starting_target_orientation: [-1, 0, 0, 0, 0, 1, 0, 1, 0]   # Eigen::Matrix3d ColMajor

rviz_simulator/include/rviz_simulator/camera.h

@@ -39,8 +39,9 @@
 #include <fstream>
 
 // for making directories
-#include <sys/stat.h>
-#include <sys/types.h>
+// #include <sys/stat.h>
+// #include <sys/types.h>
+#include <boost/filesystem.hpp>
 
 // for getting package path
 #include <ros/package.h>
@@ -116,16 +117,16 @@ class Camera : public Target
   /// @param marker_position_in_ROSWorld      Initial position of tag in ROSWorld
   /// @param marker_orientation_in_ROSWorld   Initial orientation of tag in ROSWorld
   /// @param marker_color_RGBA                Marker colour (red, green blue) and opacity setting
-  /// @param marker_scale                     The length of a side of the interactive marker
+  /// @param camera_size                      x, y, z dimensions of the camera visualization marker in RViz
+  /// @param target_size                      x, y, z dimensions a given target
   /// @param g_interactive_marker_server      Shared pointer for interactive marker server
   /// @param interaction_mode                 Specifies how the marker will react to events (3D MOVEMENT or BUTTON
   /// clicks)
-  /// @param camera_intrinsics              Intrinsic properties for the simulated camera
-  /// @param camera_distortions             Camera distortions
+  /// @param camera_properties                
   Camera(const std::string marker_frame_id, const std::string marker_name,
          const geometry_msgs::Point marker_position_in_ROSWorld,
          const geometry_msgs::Quaternion marker_orientation_in_ROSWorld, const std_msgs::ColorRGBA marker_color_RGBA,
-         double marker_scale, double target_scale,
+         std::vector<double> camera_size, std::vector<double> target_size,
          boost::shared_ptr<interactive_markers::InteractiveMarkerServer> g_interactive_marker_server,
          unsigned int interaction_mode, CameraProperties camera_properties);
 

rviz_simulator/include/rviz_simulator/target.h

@@ -46,14 +46,14 @@ class Target
   /// @param marker_position_in_ROSWorld      Initial position of tag in ROSWorld
   /// @param marker_orientation_in_ROSWorld   Initial orientation of tag in ROSWorld
   /// @param marker_color_RGBA                Marker colour (red, green blue) and opacity setting
-  /// @param marker_scale                     The length of a side of the interactive marker
+  /// @param target_size                      Vector of size 3 denoting the marker's [length, width, depth] dimensions 
   /// @param g_interactive_marker_server      Shared pointer for interactive marker server
   /// @param interaction_mode                 Specifies how the marker will react to events (3D MOVEMENT or BUTTON
   /// clicks)
   Target(const std::string marker_frame_id, const std::string marker_name,
          const geometry_msgs::Point marker_position_in_ROSWorld,
          const geometry_msgs::Quaternion marker_orientation_in_ROSWorld, const std_msgs::ColorRGBA marker_color_RGBA,
-         double marker_scale,
+         std::vector<double> target_size,
          boost::shared_ptr<interactive_markers::InteractiveMarkerServer> g_interactive_marker_server,
          unsigned int interaction_mode);
 
@@ -79,7 +79,7 @@ class Target
   geometry_msgs::Point marker_position_in_ROSWorld_;
   geometry_msgs::Quaternion marker_orientation_in_ROSWorld_;
   std_msgs::ColorRGBA marker_color_RGBA_;
-  double marker_scale_;
+  std::vector<double> target_size_;
   unsigned int interaction_mode_;
 
   /// Makes the cube representative of the marker and sets its characteristics.

rviz_simulator/launch/simulate.launch

@@ -6,10 +6,8 @@
   <!-- loading values into the ros parameter server -->
   <rosparam command="load" file="$(find rviz_simulator)/config/initialize_simulator.yaml" />
 
-  <!-- adding a command line arg for camera intrinsics file -->
   <rosparam command="load" file="$(find rviz_simulator)/config/$(arg camera_intrinsics_file)" />
 
-  <!-- adding a command line arg for debug mode -->
   <group if="$(arg debug_mode)">
     <node pkg="rqt_console" type="rqt_console" name="rqt_console"/>
     <node pkg="rqt_logger_level" type="rqt_logger_level" name="rqt_logger_level"/>

rviz_simulator/src/camera.cpp

@@ -48,19 +48,20 @@ Eigen::MatrixXd CameraProperties::getCameraMatrix()
 Camera::Camera(const std::string marker_frame_id, const std::string marker_name,
                const geometry_msgs::Point marker_position_in_ROSWorld,
                const geometry_msgs::Quaternion marker_orientation_in_ROSWorld,
-               const std_msgs::ColorRGBA marker_color_RGBA, double marker_scale, double target_scale,
+               const std_msgs::ColorRGBA marker_color_RGBA, std::vector<double> camera_size,
+               std::vector<double> target_size,
                boost::shared_ptr<interactive_markers::InteractiveMarkerServer> g_interactive_marker_server,
                unsigned int interaction_mode, CameraProperties camera_properties)
   : Target(marker_frame_id, marker_name, marker_position_in_ROSWorld, marker_orientation_in_ROSWorld, marker_color_RGBA,
-           marker_scale, g_interactive_marker_server, interaction_mode)
+           camera_size, g_interactive_marker_server, interaction_mode)
 {
   this->camera_properties_ = camera_properties;
   this->camera_properties_.populateCameraMatrix();
 
   // TODO
   // move target length properties to the target class in order to make this configurable for each target.
-  this->target_x_length_ = target_scale;
-  this->target_y_length_ = target_scale;
+  this->target_x_length_ = target_size[0];
+  this->target_y_length_ = target_size[1];
   this->initObjPointsInTarget();
 
   // adding a shutter release button to the camera
@@ -107,17 +108,17 @@ void Camera::makeOutputDirectories()
   std::string detections_root_folder = package_path + "/detections";
 
   // if detections root directory does not exist then create it
-  struct stat info;
-  if (stat(detections_root_folder.c_str(), &info) != 0)
+  // struct stat info;
+  if (!boost::filesystem::is_directory(detections_root_folder))
   {
     ROS_INFO_STREAM("Making directory: " << detections_root_folder);
-    if (mkdir(detections_root_folder.c_str(), 0777) == -1)
+    if (!boost::filesystem::create_directories(detections_root_folder))
       ROS_ERROR_STREAM("Error making directory :  " << strerror(errno) << std::endl);
   }
 
   this->output_folder_path_ = detections_root_folder + "/" + output_folder_name;
 
-  if (mkdir(this->output_folder_path_.c_str(), 0777) == -1)
+  if (!boost::filesystem::create_directories(this->output_folder_path_))
     ROS_ERROR_STREAM("Error :  " << strerror(errno) << std::endl);
 
   else
@@ -241,11 +242,11 @@ void Camera::takePicture()
   std::string output_file_path = this->output_folder_path_ + "/" + output_file_name;
   std::ofstream fout(output_file_path);
   if (!fout)
-    ROS_ERROR_STREAM("Output file error.\n" << output_file_path << "\n");
+    ROS_ERROR_STREAM("Output file error.\n" << output_file_path);
   else
   {
     fout << detections_out.c_str();
-    ROS_INFO_STREAM("Corner detections dumped to " << output_file_path << "\n");
+    ROS_INFO_STREAM("Corner detections dumped to " << output_file_path);
     fout.close();
     this->num_pictures_++;
   }
@@ -259,7 +260,7 @@ std::vector<Eigen::Vector2d> Camera::processCorners(Eigen::Affine3d camera_T_tar
   for (int i = 0; i < this->obj_points_in_target_.size(); i++)
   {
     Eigen::Vector2d corner = calculatePixelCoords(camera_T_target, this->obj_points_in_target_[i]);
-    ROS_INFO_STREAM("Corner " << i << " u: " << corner[0] << " v: " << corner[1] << std::endl);
+    ROS_INFO_STREAM("Corner " << i << " u: " << corner[0] << " v: " << corner[1]);
     if (!this->isWithinImage(corner))
       return std::vector<Eigen::Vector2d>();  // return empty vector
     corners2d.push_back(corner);
@@ -269,7 +270,7 @@ std::vector<Eigen::Vector2d> Camera::processCorners(Eigen::Affine3d camera_T_tar
   if (!this->isInRange(corners2d))
     return std::vector<Eigen::Vector2d>();  // return empty vector
 
-  ROS_INFO_STREAM("All corners valid.\n");
+  ROS_INFO_STREAM("All corners valid.");
   return corners2d;
 }
 
@@ -314,32 +315,32 @@ bool Camera::isWithinImage(Eigen::Vector2d pixelCoords)
   if (pixelCoords.x() >= 0 && pixelCoords.x() < this->camera_properties_.image_width && pixelCoords.y() >= 0 &&
       pixelCoords.y() < this->camera_properties_.image_height)
   {
-    ROS_INFO_STREAM("Corner is within image\n");
+    ROS_INFO_STREAM("Corner is within image.");
     return true;
   }
-  ROS_INFO_STREAM("Corner is outside image\n");
+  ROS_INFO_STREAM("Corner is outside image.");
   return false;
 }
 
 bool Camera::isInFrontOfCamera(Eigen::Affine3d camera_T_target)
 {
   if (camera_T_target.translation().z() > 0)
   {
-    ROS_INFO_STREAM("Target in front of camera\n");
+    ROS_INFO_STREAM("Target in front of camera.");
     return true;
   }
-  ROS_INFO_STREAM("Target behind camera\n");
+  ROS_INFO_STREAM("Target behind camera.");
   return false;
 }
 
 bool Camera::isInFrontOfTarget(Eigen::Affine3d target_T_camera)
 {
   if (target_T_camera.translation().z() > 0)
   {
-    ROS_INFO_STREAM("Camera in front of target\n");
+    ROS_INFO_STREAM("Camera in front of target.");
     return true;
   }
-  ROS_INFO_STREAM("Camera behind target\n");
+  ROS_INFO_STREAM("Camera behind target.");
   return false;
 }
 
@@ -357,11 +358,11 @@ bool Camera::isInRange(std::vector<Eigen::Vector2d> corners)
   {
     if (differences[i].norm() < this->camera_properties_.min_corner_dist)
     {
-      ROS_INFO_STREAM("Target not in range\n.");
+      ROS_INFO_STREAM("Target not in range.");
       return false;
     }
   }
-  ROS_INFO_STREAM("Target in range\n.");
+  ROS_INFO_STREAM("Target in range.");
   return true;
 }
 
@@ -412,7 +413,7 @@ void Camera::dumpCameraPropertiesToYAMLFile(std::string output_file_name)
   std::ofstream fout(camera_file_path);
   if (!fout)
   {
-    ROS_ERROR_STREAM("Output file error.\n" << camera_file_path << "\n");
+    ROS_ERROR_STREAM("Output file error.\n" << camera_file_path);
   }
   else
   {
@@ -440,7 +441,7 @@ void Camera::calculateWorld_T_Targets(std::string output_file_name)
   {
     // getting world_T_target
     std::string target_name = "tag" + std::to_string(i);
-    ROS_INFO_STREAM("Calculated and stored targetID: " << target_name << std::endl);
+    ROS_INFO_STREAM("Calculated and stored targetID: " << target_name);
     visualization_msgs::InteractiveMarker target_marker;
     this->g_interactive_marker_server_->get(target_name, target_marker);
     Eigen::Affine3d world_T_target = this->calculateReference_T_Target(world_marker, target_marker);
@@ -459,12 +460,12 @@ void Camera::calculateWorld_T_Targets(std::string output_file_name)
   std::string output_file_path = this->output_folder_path_ + "/" + output_file_name;
   std::ofstream fout(output_file_path);
   if (!fout)
-    ROS_ERROR_STREAM("Output file error.\n" << output_file_path << "\n");
+    ROS_ERROR_STREAM("Output file error.\n" << output_file_path);
   else
   {
     fout << targets_out.c_str();
-    ROS_INFO_STREAM("Target data dumped to " << output_file_path << "\n");
-    ROS_WARN("Do not move the targets in rviz from this point!\n");
+    ROS_INFO_STREAM("Target data dumped to " << output_file_path);
+    ROS_WARN("Do not move the targets in rviz from this point!");
     fout.close();
   }
 }