Update/sensor noise qual (#52)

* preliminary library WIP commit

* executable outline

* pnp3d builds

* changed derpicated struct member

* pnp unit test passes

* fixed issues form stash

* moving from image_tools to optimizations

* Builds with main file in rct_examples, and lib in rct_optimization.

* trying to test unit test

* Minimal viable for 2d, but needs significant code cleaning, 3d implemented, and some improved methods

* preliminary PR build

* PR revision pt.1. Missing gaussian noise, stat struct rework, and further documentation

* Changed test pose

* more documentation

* debugging pnp

* angle-axis representation; still fails. Switching to quaternions

* quaternion tests still fail

* Quaternion Method Functioning

* squash when things work. Temp commit: 3d pnp is very inaccurate

* 3d noise qualification fails. Occasional innacuracy, with occasional NaN returns or 60 deg oritentation shits

* squash me; commiting for rebase

* local paramterization may have solved 3d accuracy

* Removed debug prints, added pnp 3d noise test

* raised angular tolerance to 8 degrees, for xenial compatability

* removed commented code

* Revised noise qualification code

* Updated noise qualification unit test

* Renamed to noise qualification

* Revised noise qualification example

* Moved and renamed noise qualification tool

* Added example launch file for noise qualification

* raised quaternion sampling

Co-authored-by: ctlewis <colin.lewis@swri.org>
Co-authored-by: mripperger <michael.ripperger@swri.org>

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,10 @@ 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_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/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

rct_optimizations/src/rct_optimizations/pnp.cpp

@@ -1,6 +1,7 @@
 #include "rct_optimizations/pnp.h"
 #include "rct_optimizations/ceres_math_utilities.h"
 #include "rct_optimizations/local_parameterization.h"
+
 #include <ceres/ceres.h>
 
 namespace
@@ -43,10 +44,44 @@ struct SolvePnPCostFunc
   Eigen::Vector2d in_image_;
 };
 
+struct SolvePnPCostFunc3D
+{
+public:
+  SolvePnPCostFunc3D(const Eigen::Vector3d& pt_in_target,
+                     const Eigen::Vector3d& pt_in_image)
+    :in_target_(pt_in_target), in_image_(pt_in_image)
+  {}
+
+  template<typename T>
+  bool operator()(const T *const target_q, const T *const target_t, T *const residual) const
+  {
+
+    using Isometry3 = Eigen::Transform<T, 3, Eigen::Isometry>;
+    using Vector3 = Eigen::Matrix<T, 3, 1>;
+
+    Eigen::Map<const Eigen::Quaternion<T>> q(target_q);
+    Eigen::Map<const Vector3> t(target_t);
+    Isometry3 camera_to_target = Isometry3::Identity();
+    camera_to_target = Eigen::Translation<T, 3>(t) * q;
+
+    // Transform points into camera coordinates
+    Vector3 camera_pt = camera_to_target * in_target_.cast<T>();
+
+    residual[0] = camera_pt[0] - in_image_.x();
+    residual[1] = camera_pt[1] - in_image_.y();
+    residual[2] = camera_pt[2] - in_image_.z();
+    return true;
+  }
+
+  Eigen::Vector3d in_target_;
+  Eigen::Vector3d in_image_;
+};
+
 } // namespace anonymous
 
 namespace rct_optimizations
 {
+
 PnPResult optimize(const PnPProblem &params)
 {
   // Create the optimization variables from the input guess
@@ -88,5 +123,42 @@ PnPResult optimize(const PnPProblem &params)
   return result;
 }
 
-} // namespace rct_optimizations
+PnPResult optimize(const rct_optimizations::PnPProblem3D& params)
+{
+  // Create the optimization variables from the input guess
+  Eigen::Quaterniond q(params.camera_to_target_guess.rotation());
+  Eigen::Vector3d t(params.camera_to_target_guess.translation());
+
+  ceres::Problem problem;
+
+  //only one loop, for correspondences
+  for (std::size_t i = 0; i < params.correspondences.size(); ++i) // For each 3D point seen in the 3D image
+  {
+    // Define
+    const auto& img_obs = params.correspondences[i].in_image;
+    const auto& point_in_target = params.correspondences[i].in_target;
+
+    // Allocate Ceres data structures - ownership is taken by the ceres
+    // Problem data structure
+
+    auto* cost_fn = new SolvePnPCostFunc3D(point_in_target, img_obs);
+
+    auto* cost_block = new ceres::AutoDiffCostFunction<SolvePnPCostFunc3D, 2, 4, 3>(cost_fn);
+
+    problem.AddResidualBlock(cost_block, nullptr, q.coeffs().data(), t.data());
+  }
+
+  ceres::Solver::Options options;
+  //options.function_tolerance = 1e-7;
+  ceres::Solver::Summary summary;
+  ceres::Solve(options, &problem, &summary);
+
+  PnPResult result;
+  result.converged = summary.termination_type == ceres::CONVERGENCE;
+  result.initial_cost_per_obs = summary.initial_cost / summary.num_residuals;
+  result.final_cost_per_obs = summary.final_cost / summary.num_residuals;
+  result.camera_to_target = Eigen::Translation3d(t) * q;
 
+  return result;
+}
+} // namespace rct_optimizations