Camera class added and fiducial marker detections

rviz_simulator/README.md

@@ -35,7 +35,7 @@ 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 `realsense_camera_intrinsics.yaml` file is loaded. To specify another camera intrinsics file, run the launch file with the command line arg
+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
 > `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.

rviz_simulator/config/initialize_simulator.yaml

@@ -1,21 +1,13 @@
 world_frame_id: "ROSWorld"
 
 # target parameters
-
-# colors to be decoded as std_msgs::ColorRGBA 
-# stored in yaml as [red, green, blue, alpha]
-grey: [0.5, 0.5, 0.5, 1.0]
-blue: [0.0, 0.67, 0.9, 1.0]
-orange: [1.0, 0.39, 0.12, 1.0]
-
-num_targets_in_line: 5
+num_targets: 5
 distance_between_targets: 1.0
-starting_target_position: [0.0, 0.0, 0.0]   # geometery_msgs::Point
-starting_target_orientation: [0, 0, 0, 1]   # geometry_msgs::Quaternion
-target_scale: 0.1
+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
 
 # camera parameters
-camera_intrinsics_file: "realsense_camera_intrinsics.yaml" 
-starting_camera_positon: [0, 3, 0 ]   # geometery_msgs::Point
-starting_camera_orientation: [0, -0.7071068, 0.7071068, 0]  # geometry_msgs::Quaternion (roll 90, yaw 180)
-camera_scale: 0.2
+camera_intrinsics_file: "photoneo_camera_intrinsics.yaml" 
+starting_camera_position: [0, 3, 0 ]                        # geometery_msgs::Point
+starting_camera_orientation: [-1, 0, 0, 0, 0, -1, 0, -1, 0] # Eigen::Matrix3d ColMajor
+

rviz_simulator/config/photoneo_camera_intrinsics.yaml

@@ -1,6 +1,6 @@
 image_width: 1032
 image_height: 772
-camera_name: "Photoneo (Camera)"
+camera_name: "Photoneo Camera"
 camera_matrix:
   rows: 3
   cols: 3

rviz_simulator/config/realsense_camera_intrinsics.yaml

@@ -1,6 +1,6 @@
 image_width: 1280
 image_height: 720
-camera_name: "Realsense Camera (Camera)"
+camera_name: "Realsense Camera"
 near_clip: 0.1      # remove this
 far_clip: 5         # remove this
 camera_matrix:

rviz_simulator/include/rviz_simulator/camera.h

@@ -48,6 +48,11 @@
 // for Rotation Matrix to Rodrigues angle axis
 #include <ceres/rotation.h>
 
+// TODO: refactor using camera_calibration_parsers
+// http://docs.ros.org/api/camera_calibration_parsers/html/namespacecamera__calibration__parsers.html#a02669daad59f8986503dfaae4cc01867
+// for reading camera calibration YAML files
+// #include <camera_calibration_parsers/parse.h>
+
 #include "rviz_simulator/target.h"
 
 namespace rviz_simulator
@@ -72,15 +77,18 @@ struct CameraProperties
   /// camera image extremes
   int image_height;
   int image_width;
-  double min_distance_between_target_corners;  // in pixels
-  double max_distance_between_camera_and_target;
-  double min_distance_between_camera_and_target;
+  double min_corner_dist = 30;  // in pixels
+  double cam_target_max_dist;
+  double cam_target_min_dist;
 
   // camera distortion coefficients
   std::string distortion_model;
   double k1, k2, k3, k4, k5, k6;  /// radial distortions
   double p1, p2;                  /// tangential distortions
 
+  // TODO: refactor using camera_calibration_parsers
+  // Accepts a yaml file path and populates the CameraProperties struct
+
   // getter and setter for camera_matrix
   /// Function populates the camera_matrix with the camera's intrinsic values
   void populateCameraMatrix();
@@ -129,11 +137,14 @@ class Camera : public Target
 
 private:
   CameraProperties camera_properties_;  /// Camera properties
-  double target_x_length_;              /// The real-world x (horizontal) length of a target
-  double target_y_length_;              /// The real-world (vertical) length of a target
-  int num_pictures_;                    /// The number of pictures taken during the running of the node
-  std::string output_folder_path_;      /// absolute path of folder in which the detection yaml files are dumped
-  bool first_picture_taken_;            /// set to true when the first picture is taken
+
+  // TODO:
+  // move target length properties to the target class in order to make this configurable for each target.
+  double target_x_length_;          /// The real-world x (horizontal) length of a target
+  double target_y_length_;          /// The real-world y (vertical) length of a target
+  int num_pictures_;                /// The number of pictures taken during the running of the node
+  std::string output_folder_path_;  /// absolute path of folder in which the detection yaml files are dumped
+  bool first_picture_taken_;        /// set to true when the first picture is taken
   std::vector<Eigen::Affine3d> world_T_targets_;       /// considering index 0 as the world origin
   std::vector<Eigen::Vector4d> obj_points_in_target_;  /// corner order [bl, br, tr, tl]
 
@@ -168,17 +179,17 @@ class Camera : public Target
   void takePicture();
 
   /// Calculates the pixel coordinates of the four corners of a target.
-  /// @param camera_extrinsics    The transform camera_T_target.
+  /// @param camera_T_target      The transform camera_T_target.
   /// @return                     Vector of corner (x, y) pixel coordinates
-  std::vector<Eigen::Vector2d> processCorners(Eigen::Affine3d camera_extrinsics);
+  std::vector<Eigen::Vector2d> processCorners(Eigen::Affine3d camera_T_target);
 
   /// Converts the single 3D point of a target's corner in the world to a 2d pixel coordinate in an images
   /// Performs rectification on distorted camera images.
   /// Follows the model used in the OpenCV camera calibration doc
-  /// @param camera_extrinsics    The transform camera_T_target.
-  /// @param point                A 3D point of a targets corner in the world frame represented as (X, Y, Z, 1).
+  /// @param camera_T_target      The transform camera_T_target.
+  /// @param point_in_target      A 3D point of a targets corner in the world frame represented as (X, Y, Z, 1).
   /// @return                     A 2D vector (u,v) representing the pixel coordinate.
-  Eigen::Vector2d calculatePixelCoords(Eigen::Affine3d camera_extrinsics, Eigen::Vector4d point);
+  Eigen::Vector2d calculatePixelCoords(Eigen::Affine3d camera_T_target, Eigen::Vector4d point_in_target);
 
   /// Checks if a single pixel coordinate is in range.
   /// @param pixelCoords      The (x,y) coordinates of a pixel in range [0 - image_width-1] and [0 - image_height-1]

rviz_simulator/launch/simulate.launch

@@ -1,13 +1,15 @@
 <launch>
+  <!-- args -->
+  <arg name="camera_intrinsics_file" default="photoneo_camera_intrinsics.yaml"/>
+  <arg name="debug_mode" default="false"/>
+
   <!-- 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 -->
-  <arg name="camera_intrinsics_file" default="photoneo_camera_intrinsics.yaml"/>
   <rosparam command="load" file="$(find rviz_simulator)/config/$(arg camera_intrinsics_file)" />
 
   <!-- adding a command line arg for debug mode -->
-  <arg name="debug_mode" default="false"/>
   <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

@@ -37,7 +37,7 @@ void CameraProperties::populateCameraMatrix()
 {
   camera_matrix.resize(3, 4);
   camera_matrix << fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0;
-  ROS_INFO("Camera matix populated.\n");
+  ROS_INFO("Camera matix populated.");
 }
 
 Eigen::MatrixXd CameraProperties::getCameraMatrix()
@@ -57,13 +57,15 @@ Camera::Camera(const std::string marker_frame_id, const std::string marker_name,
   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->initObjPointsInTarget();
 
   // adding a shutter release button to the camera
   this->interactive_marker_.controls[0].interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
-  ROS_INFO("Camera shutter button added.\n");
+  ROS_INFO("Camera shutter button added.");
 
   this->makeOutputDirectories();
 
@@ -83,17 +85,17 @@ Camera::~Camera()
 void Camera::initObjPointsInTarget()
 {
   Eigen::Vector4d c0, c1, c2, c3;
-  c0 << -this->target_x_length_/ 2, -this->target_y_length_/ 2, 0, 1;  // old c0
-  c3 << -this->target_x_length_/ 2, this->target_y_length_/ 2, 0, 1;   // old c1
-  c2 << this->target_x_length_/ 2, this->target_y_length_/ 2, 0, 1;    // old c2
-  c1 << this->target_x_length_/ 2, -this->target_y_length_/ 2, 0, 1;   // old c3
+  c0 << -this->target_x_length_ / 2, -this->target_y_length_ / 2, 0, 1;  // old c0
+  c3 << -this->target_x_length_ / 2, this->target_y_length_ / 2, 0, 1;   // old c1
+  c2 << this->target_x_length_ / 2, this->target_y_length_ / 2, 0, 1;    // old c2
+  c1 << this->target_x_length_ / 2, -this->target_y_length_ / 2, 0, 1;   // old c3
 
   this->obj_points_in_target_.push_back(c0);
   this->obj_points_in_target_.push_back(c1);
   this->obj_points_in_target_.push_back(c2);
   this->obj_points_in_target_.push_back(c3);
 
-  ROS_INFO("Object points in target calculated.\n");
+  ROS_INFO("Object points in target calculated.");
 }
 
 void Camera::makeOutputDirectories()
@@ -119,15 +121,15 @@ void Camera::makeOutputDirectories()
     ROS_ERROR_STREAM("Error :  " << strerror(errno) << std::endl);
 
   else
-    ROS_INFO_STREAM("Output directory created sucessfully: " << this->output_folder_path_.c_str() << std::endl);
+    ROS_INFO_STREAM("Output directory created sucessfully: " << this->output_folder_path_.c_str());
 }
 
 void Camera::addCameraToServer()
 {
   this->g_interactive_marker_server_->insert(this->interactive_marker_);
   this->g_interactive_marker_server_->setCallback(this->interactive_marker_.name,
                                                   boost::bind(&rviz_simulator::Camera::cameraFeedback, this, _1));
-  ROS_INFO_STREAM("Camera added to interactive marker server.\n");
+  ROS_INFO_STREAM("Camera added to interactive marker server.");
 }
 
 void Camera::cameraFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
@@ -216,12 +218,15 @@ void Camera::takePicture()
       {
         detections_out << YAML::BeginMap;  // target
         detections_out << YAML::Key << "targetID" << YAML::Value << std::to_string(i);
+        std::vector<double> size = { this->target_x_length_, this->target_y_length_ };
+        detections_out << YAML::Key << "size" << YAML::Value << YAML::Flow << size;
         detections_out << YAML::Key << "corners";
         detections_out << YAML::Value << YAML::BeginMap;  // corners
         for (int i = 0; i < corners.size(); i++)
         {
+          std::vector<double> corner = { corners[i].x(), corners[i].y() };
           detections_out << YAML::Key << i;
-          detections_out << YAML::Value << YAML::BeginSeq << int(corners[i][0]) << int(corners[i][1]) << YAML::EndSeq;
+          detections_out << YAML::Value << YAML::Flow << corner;
         }
         detections_out << YAML::EndMap;  // corners
         detections_out << YAML::EndMap;  // target
@@ -268,9 +273,9 @@ std::vector<Eigen::Vector2d> Camera::processCorners(Eigen::Affine3d camera_T_tar
   return corners2d;
 }
 
-Eigen::Vector2d Camera::calculatePixelCoords(Eigen::Affine3d camera_T_target, Eigen::Vector4d point_in_tag)
+Eigen::Vector2d Camera::calculatePixelCoords(Eigen::Affine3d camera_T_target, Eigen::Vector4d point_in_target)
 {
-  Eigen::Vector4d point_in_camera = camera_T_target * point_in_tag;
+  Eigen::Vector4d point_in_camera = camera_T_target * point_in_target;
 
   // scaling
   double x_prime = (point_in_camera[0] / point_in_camera[2]);
@@ -309,10 +314,10 @@ 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("Valid corner coordinate\n");
+    ROS_INFO_STREAM("Corner is within image\n");
     return true;
   }
-  ROS_INFO_STREAM("Invalid corner coordinate\n");
+  ROS_INFO_STREAM("Corner is outside image\n");
   return false;
 }
 
@@ -329,7 +334,7 @@ bool Camera::isInFrontOfCamera(Eigen::Affine3d camera_T_target)
 
 bool Camera::isInFrontOfTarget(Eigen::Affine3d target_T_camera)
 {
-  if (target_T_camera.translation().y() > 0)
+  if (target_T_camera.translation().z() > 0)
   {
     ROS_INFO_STREAM("Camera in front of target\n");
     return true;
@@ -350,7 +355,7 @@ bool Camera::isInRange(std::vector<Eigen::Vector2d> corners)
 
   for (int i = 0; i < differences.size(); i++)
   {
-    if (differences[i].norm() < this->camera_properties_.min_distance_between_target_corners)
+    if (differences[i].norm() < this->camera_properties_.min_corner_dist)
     {
       ROS_INFO_STREAM("Target not in range\n.");
       return false;
@@ -374,11 +379,13 @@ void Camera::dumpCameraPropertiesToYAMLFile(std::string output_file_name)
   out << YAML::BeginMap;  // camera matrix map
   out << YAML::Key << "rows" << YAML::Value << 3;
   out << YAML::Key << "cols" << YAML::Value << 3;
-  out << YAML::Key << "data" << YAML::Value;
-  out << YAML::BeginSeq;  // camera matrix data sequence
-  out << camera_properties_.fx << 0 << camera_properties_.cx << 0 << camera_properties_.fy << camera_properties_.cy << 0
-      << 0 << 1;
-  out << YAML::EndSeq;  // camera matrix data sequence
+
+  // RowMajor
+  std::vector<double> camera_matrix_data = {
+    camera_properties_.fx, 0, camera_properties_.cx, 0, camera_properties_.fy, camera_properties_.cy, 0, 0, 1
+  };
+
+  out << YAML::Key << "data" << YAML::Value << YAML::Flow << camera_matrix_data;
   out << YAML::EndMap;  // camera matrix map
 
   // distortions
@@ -387,11 +394,11 @@ void Camera::dumpCameraPropertiesToYAMLFile(std::string output_file_name)
   out << YAML::BeginMap;  // distortion coefficitents map
   out << YAML::Key << "rows" << YAML::Value << 1;
   out << YAML::Key << "cols" << YAML::Value << 5;
-  out << YAML::Key << "data" << YAML::Value;
-  out << YAML::BeginSeq;  // distortion coefficients data sequence
-  out << camera_properties_.k1 << camera_properties_.k2 << camera_properties_.p1 << camera_properties_.p2
-      << camera_properties_.k3;
-  out << YAML::EndSeq;  // distortion coefficients data sequence
+
+  std::vector<double> distortion_coefficients = { camera_properties_.k1, camera_properties_.k2, camera_properties_.p1,
+                                                  camera_properties_.p2, camera_properties_.k3 };
+
+  out << YAML::Key << "data" << YAML::Value << YAML::Flow << distortion_coefficients;
   out << YAML::EndMap;  // distortion coefficitents map
 
   // rectification matrix (omitted)
@@ -410,7 +417,7 @@ void Camera::dumpCameraPropertiesToYAMLFile(std::string output_file_name)
   else
   {
     fout << out.c_str();
-    ROS_INFO_STREAM("Camera properties dumped to " << camera_file_path << "\n");
+    ROS_INFO_STREAM("Camera properties dumped to " << camera_file_path);
   }
   fout.close();
 }
@@ -490,7 +497,8 @@ void Camera::ObjPointsInTargetToYAML(YAML::Emitter& out)
   for (int i = 0; i < this->obj_points_in_target_.size(); i++)
   {
     Eigen::Vector4d point = this->obj_points_in_target_[i];
-    out << YAML::Key << i << YAML::Value << YAML::BeginSeq << point.x() << point.y() << point.z() << YAML::EndSeq;
+    std::vector<double> p = { point.x(), point.y(), point.z() };
+    out << YAML::Key << i << YAML::Value << YAML::Flow << p;
   }
   out << YAML::EndMap;
 }