WIP: Update/proclivity

rct_examples/CMakeLists.txt

@@ -124,6 +124,15 @@ add_dependencies(${PROJECT_NAME}_camera_intrinsic_calibration_validation ${${PRO
 target_link_libraries(${PROJECT_NAME}_camera_intrinsic_calibration_validation ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
 
 
+#Executable for testing camera noise
+add_executable(${PROJECT_NAME}_noise_qualification_2d src/tools/noise_qualification_2d.cpp)
+
+set_target_properties(${PROJECT_NAME}_noise_qualification_2d PROPERTIES OUTPUT_NAME noise_qualification_2d PREFIX "")
+
+add_dependencies(${PROJECT_NAME}_noise_qualification_2d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(${PROJECT_NAME}_noise_qualification_2d ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
+
 #############
 ## Testing ##
 #############
@@ -145,6 +154,7 @@ install(TARGETS
     ${PROJECT_NAME}_intr
     ${PROJECT_NAME}_pnp
     ${PROJECT_NAME}_camera_intrinsic_calibration_validation
+    ${PROJECT_NAME}_noise_qualification_2d
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

rct_examples/config/noise_qualification/camera_to_target_guess.yaml

@@ -0,0 +1,8 @@
+camera_to_target_guess:
+  x: 0.00330134
+  y: 0.0778156
+  z: 0.77218
+  qx: 0.999656
+  qy: 0.00396669
+  qz: 0.020924
+  qw: 0.0152998

rct_examples/config/noise_qualification/data.yaml

@@ -0,0 +1,16 @@
+# List of images relative to a data directory
+# In this example case all the images are the same to produce a zero-noise result
+- image: images/0.png
+- image: images/0.png
+- image: images/0.png
+- image: images/0.png
+- image: images/0.png
+- image: images/0.png
+- image: images/0.png
+- image: images/0.png
+- image: images/0.png
+- image: images/0.png
+- image: images/0.png
+- image: images/0.png
+- image: images/0.png
+- image: images/0.png

rct_examples/launch/noise_qualification_2d.launch

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="data_path" default="$(find rct_examples)/data/test_set_10x10"/>
+  <arg name="data_file" default="$(find rct_examples)/config/noise_qualification/data.yaml"/>
+  <arg name="pose_file" default="$(find rct_examples)/config/noise_qualification/camera_to_target_guess.yaml"/>
+  <arg name="target_file" default="$(find rct_examples)/config/target_10x10.yaml"/>
+  <arg name="camera_intr_file" default="$(find rct_examples)/config/kinect_camera_intr.yaml"/>
+
+  <node name="noise_qualification_2d" pkg="rct_examples" type="noise_qualification_2d" output="screen">
+    <param name="data_path" value="$(arg data_path)"/>
+    <param name="data_file" value="$(arg data_file)"/>
+    <rosparam command="load" file="$(arg pose_file)"/>
+    <rosparam command="load" file="$(arg target_file)"/>
+    <rosparam command="load" file="$(arg camera_intr_file)"/>
+  </node>
+</launch>

rct_examples/src/tools/noise_qualification_2d.cpp

@@ -0,0 +1,109 @@
+#include <opencv/cv.hpp>
+#include <rct_image_tools/image_observation_finder.h>
+#include <rct_optimizations/validation/noise_qualification.h>
+#include "rct_ros_tools/data_set.h"
+#include "rct_ros_tools/parameter_loaders.h"
+#include <ros/ros.h>
+#include <yaml-cpp/yaml.h>
+
+template<typename T>
+bool get(const ros::NodeHandle &nh, const std::string &key, T &val)
+{
+  if (!nh.getParam(key, val))
+  {
+    ROS_ERROR_STREAM("Failed to get '" << key << "' parameter");
+    return false;
+  }
+  return true;
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "noise_qualification_2d");
+  ros::NodeHandle pnh("~");
+
+  // Parse parameters
+  std::string data_path;
+  if (!get(pnh, "data_path", data_path))
+    return -1;
+
+  std::string data_file;
+  if (!get(pnh, "data_file", data_file))
+    return -1;
+
+  try
+  {
+    // Load the target definition and observation finder
+    rct_image_tools::ModifiedCircleGridTarget target = rct_ros_tools::loadTarget(pnh, "target_definition");
+    rct_image_tools::ModifiedCircleGridObservationFinder obs_finder(target);
+
+    // Load camera intrinsics
+    rct_optimizations::CameraIntrinsics camera = rct_ros_tools::loadIntrinsics(pnh, "intrinsics");
+
+    // Load an initial guess for the camera to target transformation
+    Eigen::Isometry3d camera_to_target_guess = rct_ros_tools::loadPose(pnh, "camera_to_target_guess");
+
+    // Load the data file which specifies the location of the images on which to perform the noise qualification
+    YAML::Node root = YAML::LoadFile(data_file);
+
+    // Set up the noise qualification inputs
+    std::vector<rct_optimizations::PnPProblem> problem_set;
+    problem_set.reserve(root.size());
+    for (std::size_t i = 0; i < root.size(); ++i)
+    {
+      // Each entry should have an image path. This path is relative to the root_path directory!
+      const auto img_path = root[i]["image"].as<std::string>();
+      const std::string image_name = data_path + "/" + img_path;
+      static cv::Mat image = rct_ros_tools::readImageOpenCV(image_name);
+
+      // Find the observations in the image
+      auto maybe_obs = obs_finder.findObservations(image);
+      if (!maybe_obs)
+      {
+        cv::imshow("points", image);
+        ROS_INFO_STREAM("Hit enter in the OpenCV window to continue");
+        cv::waitKey();
+        continue;
+      }
+      else
+      {
+        // Show the points we detected
+        cv::imshow("points", obs_finder.drawObservations(image, *maybe_obs));
+        ROS_INFO_STREAM("Hit enter in the OpenCV window to continue");
+        cv::waitKey();
+      }
+
+      // Set up the PnP problem for this image
+      rct_optimizations::PnPProblem problem;
+      problem.intr = camera;
+      problem.camera_to_target_guess = camera_to_target_guess;
+
+      // Add the detected correspondences
+      problem.correspondences.reserve(maybe_obs->size());
+      for (std::size_t j = 0; j < maybe_obs->size(); ++j)
+      {
+        problem.correspondences.emplace_back(maybe_obs->at(j), target.points.at(j));
+      };
+
+      problem_set.push_back(problem);
+    }
+
+    // Perform the noise qualification
+    rct_optimizations::PnPNoiseStat result = rct_optimizations::qualifyNoise2D(problem_set);
+
+    // Print the results
+    Eigen::IOFormat fmt(4, 0, ",", "\n", "[", "]");
+    ROS_INFO_STREAM("Camera to Target Noise Results");
+    ROS_INFO_STREAM("Position mean (m)\n" << result.p_stat.mean.transpose().format(fmt));
+    ROS_INFO_STREAM("Position standard deviation (m)\n" << result.p_stat.stdev.transpose().format(fmt));
+    ROS_INFO_STREAM("Quaternion mean (qx, qy, qz, qw)\n" << result.q_stat.mean.coeffs().transpose().format(fmt));
+    ROS_INFO_STREAM("Quaternion standard deviation\n" << result.q_stat.stdev);
+  }
+  catch (const std::exception &ex)
+  {
+    ROS_ERROR_STREAM(ex.what());
+    return -1;
+  }
+
+  return 0;
+}

rct_optimizations/CMakeLists.txt

@@ -24,7 +24,12 @@ add_library(${PROJECT_NAME} SHARED
   src/${PROJECT_NAME}/dh_chain.cpp
   # Validation Tools
   src/${PROJECT_NAME}/validation/camera_intrinsic_calibration_validation.cpp
+  #Noise Qualification
+  src/${PROJECT_NAME}/validation/noise_qualification.cpp
+  #Target Homography
+  src/${PROJECT_NAME}/validation/homography_check.cpp
 )
+
 target_compile_options(${PROJECT_NAME} PUBLIC -std=c++14)
 target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra)
 target_include_directories(${PROJECT_NAME} PUBLIC

rct_optimizations/include/rct_optimizations/pnp.h

@@ -14,6 +14,13 @@ struct PnPProblem
   Eigen::Isometry3d camera_to_target_guess;
 };
 
+struct PnPProblem3D
+{
+  Correspondence3D3D::Set correspondences;
+
+  Eigen::Isometry3d camera_to_target_guess;
+};
+
 struct PnPResult
 {
   bool converged;
@@ -24,6 +31,7 @@ struct PnPResult
 };
 
 PnPResult optimize(const PnPProblem& params);
+PnPResult optimize(const PnPProblem3D& params);
 
 }
 

rct_optimizations/include/rct_optimizations/validation/homography_check.h

@@ -0,0 +1,24 @@
+#pragma once
+#include <rct_optimizations/types.h>
+
+namespace rct_optimizations
+{
+/**
+ * @brief checkObservationSequence verifies that the circles on a calibration
+ * target are sequenced correctly using Homography, the mapping of image
+ * points to a plane in the image. This method assumes that the target is
+ * planar and that all target points are visible in the observation. It is the
+ *  user's responsibility to provide a reasonable error threshold.
+ * @param intr Camera intrinsics
+ * @param target Target desciption; rows, cols, and point spacing
+ * @param ob Observation set
+ * @param max_residual Maximium expected error
+ * @return A boolian indicating wheather the target points are sequenced
+ * corectly in the observation
+ */
+bool checkObservationSequence(const int& rows,
+                              const int& cols,
+                              const rct_optimizations::Observation2D3D& ob,
+                              const double& max_residual);
+
+}//rct_optimizations

rct_optimizations/include/rct_optimizations/validation/noise_qualification.h

@@ -0,0 +1,70 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include "rct_optimizations/types.h"
+#include "rct_optimizations/pnp.h"
+
+namespace rct_optimizations
+{
+/**
+ * @brief Holds the mean and standard deviation of a position vector
+ */
+struct PositionStats
+{
+  Eigen::Vector3d mean;
+  Eigen::Vector3d stdev;
+};
+
+/**
+* @brief Contains the mean and standard deviation of a quaternion orientation
+*/
+struct QuaternionStats
+{
+  Eigen::Quaterniond mean;
+  double stdev;
+};
+
+/**
+* @brief Noise statistics for a position vector and quaternion orientation
+*/
+struct PnPNoiseStat
+{
+  PositionStats p_stat;
+  QuaternionStats q_stat;
+};
+
+/**
+ * @brief Computes the mean and standard deviation of a set of quaternions
+ * @param quaternions
+ * @return
+ */
+QuaternionStats computeQuaternionStats(const std::vector<Eigen::Quaterniond> &quaternions);
+
+/**
+ * @brief Computes the mean of a set of quaternions
+ * @param orientations
+ * @return
+ */
+Eigen::Quaterniond computeQuaternionMean(const std::vector<Eigen::Quaterniond>& orientations);
+
+/**
+ * @brief This function qualifies 2D sensor noise by
+ * comparing PnP results from images taken at same pose.
+ * Sensor noise can be understood by inspecting the returned standard
+ * deviations.
+ * @param Sets of PnP 2D problem parameters
+ * @return Noise Statistics: a vector of means & std devs
+ */
+PnPNoiseStat qualifyNoise2D(const std::vector<PnPProblem>& params);
+
+/**
+ * @brief This function qualifies 3D sensor noise by
+ * comparing PnP results from scans taken at the same pose.
+ * Sensor noise can be understood by inspecting the returned standard
+ * deviations.
+ * @param params 3D image parameters
+ * @return Noise Statiscics: a vector of standard deviations and the mean pos
+ */
+PnPNoiseStat qualifyNoise3D(const std::vector<PnPProblem3D>& params);
+
+} // namespace rct_optimizations