Merge branch 'refactor' into 'master'

Refactor

See merge request !1

.clang-format

+---
+BasedOnStyle: Google
+AccessModifierOffset: -2
+ConstructorInitializerIndentWidth: 2
+AlignEscapedNewlinesLeft: false
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: None
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakTemplateDeclarations: true
+AlwaysBreakBeforeMultilineStrings: false
+BreakBeforeBinaryOperators: false
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: true
+BinPackParameters: true
+ColumnLimit: 120
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+DerivePointerBinding: false
+PointerBindsToType: true
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 60
+PenaltyBreakString: 1
+PenaltyBreakFirstLessLess: 1000
+PenaltyExcessCharacter: 1000
+PenaltyReturnTypeOnItsOwnLine: 90
+SpacesBeforeTrailingComments: 2
+Cpp11BracedListStyle: false
+Standard: Auto
+IndentWidth: 2
+TabWidth: 2
+UseTab: Never
+IndentFunctionDeclarationAfterType: false
+SpacesInParentheses: false
+SpacesInAngles: false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterControlStatementKeyword: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
+SortIncludes: false
+SpaceAfterCStyleCast: false
+
+# Configure each individual brace in BraceWrapping
+BreakBeforeBraces: Custom
+
+# Control of individual brace wrapping cases
+BraceWrapping: {
+    AfterClass: 'true'
+    AfterControlStatement: 'true'
+    AfterEnum : 'true'
+    AfterFunction : 'true'
+    AfterNamespace : 'true'
+    AfterStruct : 'true'
+    AfterUnion : 'true'
+    BeforeCatch : 'true'
+    BeforeElse : 'true'
+    IndentBraces : 'false'
+}
+...

CMakeLists.txt

 cmake_minimum_required(VERSION 2.8.3)
 project(cam_lidar_calibration)
-list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
-find_package(catkin REQUIRED COMPONENTS nodelet 
-	roscpp 
-	std_msgs 
-        pcl_conversions 
-        pcl_ros
-	image_transport
-        cv_bridge
-	sensor_msgs
-        message_generation
-        cmake_modules
-        dynamic_reconfigure)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
+
+find_package(catkin REQUIRED COMPONENTS
+  actionlib
+  actionlib_msgs
+  cv_bridge
+  dynamic_reconfigure
+  eigen_conversions
+  image_transport
+  geometry_msgs
+  message_generation
+  nodelet
+  pcl_conversions
+  pcl_ros
+  roscpp
+  rviz
+  sensor_msgs
+  std_msgs
+  )
 
 find_package(OpenCV REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system)
-find_package(Eigen REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(Qt5 REQUIRED Core Widgets)
+
 ## Setup include directories
 include_directories(
-        ${catkin_INCLUDE_DIRS}
-        include/${PROJECT_NAME}
-        ${OpenCV_INCLUDE_DIRS}
-        ${Eigen_INCLUDE_DIRS}
-        src
-)
-add_message_files(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  ${Eigen_INCLUDE_DIRS}
+  )
+#link_directories(${catkin_LIBRARY_DIRS})
+
+add_definitions(-DQT_NO_KEYWORDS)
+
+set(CMAKE_AUTOMOC ON)
+
+#add_message_files(
+#  FILES
+#  extrinsics.msg
+#  )
+add_action_files(
   FILES
-  calibration_data.msg
-  extrinsics.msg
-)
+  RunOptimise.action
+  )
+add_service_files(
+  FILES
+  Optimise.srv
+  )
 generate_messages(
   DEPENDENCIES
+  actionlib_msgs
+  geometry_msgs
   std_msgs
   sensor_msgs
-)
+  )
 generate_dynamic_reconfigure_options(
   cfg/bounds.cfg
-)
+  )
 catkin_package(
-  CATKIN_DEPENDS nodelet 
-	roscpp 
-	std_msgs 
-	sensor_msgs
-	message_runtime
-)
-
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-
-add_executable(input_sample src/input_sample.cpp)
-target_link_libraries(input_sample ${catkin_LIBRARIES})
+  CATKIN_DEPENDS nodelet
+  roscpp
+  std_msgs
+  sensor_msgs
+  message_runtime
+  )
 
-add_executable(optimizer src/optimizer.cpp)
-target_link_libraries(optimizer ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Eigen_LIBRARIES})
-add_dependencies(optimizer cam_lidar_calibration_generate_messages_cpp)
+add_library(cam_lidar_calibration
+  src/cam_lidar_panel.cpp
+  src/feature_extractor.cpp
+  src/load_params.cpp
+  src/optimiser.cpp
+  )
+target_link_libraries(cam_lidar_calibration
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBS}
+  ${Eigen_LIBRARIES}
+  Qt5::Widgets
+  )
 
-## Create the nodelet tutorial library
-add_executable(feature_extraction src/main.cpp)
-add_dependencies(feature_extraction ${PROJECT_NAME}_gencfg)
+add_executable(feature_extraction_node
+  src/feature_extraction_node.cpp
+  )
+target_link_libraries(feature_extraction_node cam_lidar_calibration ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Eigen_LIBRARIES})
 
-target_link_libraries(feature_extraction ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Eigen_LIBRARIES})
-if(catkin_EXPORTED_LIBRARIES)
-  add_dependencies(feature_extraction ${catkin_EXPORTED_LIBRARIES})
-endif()
 
 ## Mark the nodelet library for installations
-install(TARGETS feature_extraction optimizer input_sample
+install(TARGETS
+  feature_extraction_node
+  cam_lidar_calibration
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 ## Mark other files for installation (e.g. launch and bag files, etc.)
-install(FILES nodelet_definition.xml 
+install(FILES nodelet_definition.xml plugin_description.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   FILES_MATCHING PATTERN "*.h"
-)
+  )
 
 install(DIRECTORY launch/
-    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-)
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  )
 
 install(DIRECTORY cfg/
-		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg
-)
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg
+  )
 
 install(DIRECTORY rviz/
-		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz
-		)
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz
+  )
+
+## Add gtest based cpp test target and link libraries
+catkin_add_gtest(${PROJECT_NAME}-test
+  test/test_optimiser.cpp)
+if(TARGET ${PROJECT_NAME}-test)
+  target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+endif()

action/RunOptimise.action

+# Goal
+---
+# Result
+geometry_msgs/Transform transform
+---
+# Feedback

cfg/initial_params.txt

-/gmsl/A2/image_color
-/velodyne/front/points
-1
-16
-8 6
-65
-905 600
-20 0
-1178.654264230649 0.0 960.769818463969
-0.0 1182.0760126922614 603.9601849872713
-0.0 0.0 1.0
-4
--0.09405418270319424 0.08610743090764036 -0.17081566190501285 0.09818990340541457
-1920 1208

cfg/params.yaml

+feature_extraction:
+  x_min: 0.24
+  x_max: 5.0
+  y_min: -2.55
+  y_max: 1.0
+  z_min: -0.7
+  z_max: 2.0
+fisheye_model: true
+chessboard:
+  pattern_size:
+    height: 8
+    width: 6
+  square_length: 65
+  board_dimension:
+    width: 600
+    height: 905
+  translation_error:
+    x: 30
+    y: 10
+cameramat: [1178.654264230649, 0.0, 960.769818463969, 0.0, 1182.0760126922614, 603.9601849872713, 0.0, 0.0, 1.0]
+distcoeff: [-0.09405418270319424, 0.08610743090764036, -0.17081566190501285, 0.09818990340541457]
+image_size:
+  l: 1920
+  b: 1208

include/cam_lidar_calibration/feature_extractor.h

+#include <cv_bridge/cv_bridge.h>
+#include <dynamic_reconfigure/server.h>
+#include <geometry_msgs/Quaternion.h>
+#include <image_transport/image_transport.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/PointCloud2.h>
+
+#include <opencv2/core/mat.hpp>
+#include <opencv2/core/persistence.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/ModelCoefficients.h>
+
+#include <actionlib/server/simple_action_server.h>
+#include <cam_lidar_calibration/RunOptimiseAction.h>
+#include <cam_lidar_calibration/Optimise.h>
+
+#include <cam_lidar_calibration/boundsConfig.h>
+
+#include "cam_lidar_calibration/load_params.h"
+#include "cam_lidar_calibration/optimiser.h"
+#include "cam_lidar_calibration/point_xyzir.h"
+
+typedef message_filters::Subscriber<sensor_msgs::Image> image_sub_type;
+typedef message_filters::Subscriber<pcl::PointCloud<pcl::PointXYZIR>> pc_sub_type;
+
+namespace cam_lidar_calibration
+{
+geometry_msgs::Quaternion normalToQuaternion(const cv::Point3d& normal);
+
+class FeatureExtractor
+{
+public:
+  FeatureExtractor();
+  ~FeatureExtractor() = default;
+
+  void extractRegionOfInterest(const sensor_msgs::Image::ConstPtr& img,
+                               const pcl::PointCloud<pcl::PointXYZIR>::ConstPtr& pc);
+  bool serviceCB(Optimise::Request& req, Optimise::Response& res);
+  void boundsCB(boundsConfig& config, uint32_t level);
+
+  void optimise(const RunOptimiseGoalConstPtr& goal,
+                actionlib::SimpleActionServer<cam_lidar_calibration::RunOptimiseAction>* as);
+
+  void visualiseSamples();
+
+private:
+  void passthrough(const pcl::PointCloud<pcl::PointXYZIR>::ConstPtr& input_pc,
+                   pcl::PointCloud<pcl::PointXYZIR>::Ptr& output_pc);
+  std::tuple<std::vector<cv::Point3d>, cv::Mat> locateChessboard(const sensor_msgs::Image::ConstPtr& image);
+  auto chessboardProjection(const std::vector<cv::Point2d>& corners, const cv_bridge::CvImagePtr& cv_ptr);
+  void publishBoardPointCloud();
+
+  std::tuple<pcl::PointCloud<pcl::PointXYZIR>::Ptr, cv::Point3d>
+  extractBoard(const pcl::PointCloud<pcl::PointXYZIR>::Ptr& cloud);
+  std::pair<pcl::ModelCoefficients, pcl::ModelCoefficients>
+  findEdges(const pcl::PointCloud<pcl::PointXYZIR>::Ptr& edge_pair_cloud);
+
+  std::shared_ptr<Optimiser> optimiser_;
+  initial_parameters_t i_params;
+  int cb_l, cb_b, l, b, e_l, e_b;
+  std::string lidar_frame_;
+
+  int flag = 0;
+  cam_lidar_calibration::boundsConfig bounds_;
+
+  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, pcl::PointCloud<pcl::PointXYZIR>>
+      ImageLidarSyncPolicy;
+  std::shared_ptr<image_sub_type> image_sub_;
+  std::shared_ptr<pc_sub_type> pc_sub_;
+  std::shared_ptr<message_filters::Synchronizer<ImageLidarSyncPolicy>> image_pc_sync_;
+  int queue_rate_ = 5;
+
+  std::vector<pcl::PointCloud<pcl::PointXYZIR>::Ptr> pc_samples_;
+  ros::Publisher board_cloud_pub_, bounded_cloud_pub_;
+  ros::Publisher samples_pub_;
+  image_transport::Publisher image_publisher;
+  ros::ServiceServer optimise_service_;
+
+  boost::shared_ptr<image_transport::ImageTransport> it_;
+  boost::shared_ptr<image_transport::ImageTransport> it_p_;
+  boost::shared_ptr<dynamic_reconfigure::Server<cam_lidar_calibration::boundsConfig>> server;
+};
+
+}  // namespace cam_lidar_calibration

include/cam_lidar_calibration/load_params.h

+#ifndef load_params_h_
+#define load_params_h_
+
+#include <ros/ros.h>
+
+#include <opencv2/core/mat.hpp>
+
+namespace cam_lidar_calibration
+{
+struct initial_parameters_t
+{
+  bool fisheye_model;
+  int lidar_ring_count = 0;
+  cv::Size chessboard_pattern_size;
+  int square_length;                 // in millimetres
+  cv::Size board_dimensions;         // in millimetres
+  cv::Point3d cb_translation_error;  // in millimetres
+  cv::Mat cameramat;
+  cv::Mat distcoeff;
+  std::pair<int, int> image_size;  // in pixels
+};
+
+void loadParams(const ros::NodeHandle& n, initial_parameters_t& i_params);
+
+}  // namespace cam_lidar_calibration
+
+#endif

include/cam_lidar_calibration/main.h

-#include "point_xyzir.h"
-#include <ros/ros.h>
-#include <pluginlib/class_list_macros.h>
-#include <nodelet/nodelet.h>
-#include <pcl_ros/point_cloud.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <pcl_conversions/pcl_conversions.h>
-#include <pcl/point_cloud.h>
-#include <pcl/ModelCoefficients.h>
-#include <pcl/sample_consensus/method_types.h>
-#include <pcl/sample_consensus/model_types.h>
-#include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/segmentation/impl/sac_segmentation.hpp>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/filters/impl/extract_indices.hpp>
-#include <pcl/filters/project_inliers.h>
-#include <pcl/filters/impl/project_inliers.hpp> 
-#include <pcl/common/common_headers.h>
-#include <pcl/common/intersections.h>
-#include <message_filters/subscriber.h>
-#include <message_filters/synchronizer.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <sensor_msgs/Image.h>
-#include <std_msgs/Int8.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-#include <sensor_msgs/image_encodings.h>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/core/core.hpp>
-#include <opencv/cv.hpp>
-#include <opencv2/calib3d.hpp>
-#include <boost/bind.hpp>
-#include <pcl/filters/passthrough.h>
-#include <pcl/filters/impl/passthrough.hpp>
-#include "cam_lidar_calibration/calibration_data.h"
-#include <opencv2/highgui/highgui.hpp>
-#include <visualization_msgs/Marker.h>
-#include <geometry_msgs/Point.h>
-#include <math.h>
-#include <Eigen/Dense>
-#include <tf/transform_broadcaster.h>
-#include <tf/transform_datatypes.h>
-#include "pcl_ros/transforms.h"
-#include <string> 
-#include "opencv2/aruco.hpp"
-#include <fstream>
-#include <dynamic_reconfigure/server.h>
-#include <cam_lidar_calibration/boundsConfig.h>
-#include <ros/package.h>
-#include <iostream>
-#include <utility>
-#include <boost/thread.hpp>
-
-using namespace cv;
-typedef message_filters::Subscriber<sensor_msgs::Image> image_sub_type;         
-typedef message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub_type; 
-
-namespace extrinsic_calibration
-{
-
-class feature_extraction : public nodelet::Nodelet
-{
-public:
-
-  feature_extraction();
-  void extractROI(const sensor_msgs::Image::ConstPtr &img, const sensor_msgs::PointCloud2::ConstPtr &pc);
-  void flag_cb(const std_msgs::Int8::ConstPtr& msg);
-  void bounds_callback(cam_lidar_calibration::boundsConfig &config, uint32_t level);
-  double * converto_imgpts(double x, double y, double z);
-
-  void bypass_init() {
-    this->onInit();
-  }
-
-private:
-
-  struct initial_parameters
-  {
-    std::string camera_topic;
-    std::string lidar_topic;
-    bool fisheye_model;
-    int lidar_ring_count;
-    std::pair<int,int> grid_size;
-    int square_length; // in millimetres
-    std::pair<int,int> board_dimension; // in millimetres
-    std::pair<int,int> cb_translation_error; // in millimetres
-    cv::Mat cameramat;
-    int distcoeff_num;
-    cv::Mat distcoeff;
-  }i_params;
-
-  std::string pkg_loc;
-  int cb_l, cb_b, l, b, e_l, e_b;
-  double diagonal;
-
-  virtual void onInit();
-
-  ros::Publisher pub;
-  cv::FileStorage fs;
-  int flag = 0;
-  cam_lidar_calibration::boundsConfig bound;
-  cam_lidar_calibration::calibration_data sample_data;
-  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
-  message_filters::Synchronizer<MySyncPolicy> *sync;
-  int queue_rate = 5;
-
-  ros::Publisher roi_publisher;
-  ros::Publisher pub_cloud, expt_region;
-  ros::Publisher vis_pub, visPub;
-  image_transport::Publisher image_publisher;
-  ros::Subscriber flag_subscriber;
-  message_filters::Subscriber<sensor_msgs::Image> *image_sub;
-  message_filters::Subscriber<sensor_msgs::PointCloud2> *pcl_sub;
-  visualization_msgs::Marker marker;
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  boost::shared_ptr<image_transport::ImageTransport> it_p_;
-  boost::shared_ptr<dynamic_reconfigure::Server<cam_lidar_calibration::boundsConfig>> server;
-};
-
-//PLUGINLIB_EXPORT_CLASS(extrinsic_calibration::feature_extraction, nodelet::Nodelet);
-}

include/cam_lidar_calibration/optimiser.h

+#ifndef optimiser_h_
+#define optimiser_h_
+
+#include <opencv/cv.hpp>
+
+#include <ros/ros.h>
+
+#include "cam_lidar_calibration/load_params.h"
+#include "cam_lidar_calibration/openga.h"
+
+namespace cam_lidar_calibration
+{
+struct Rotation
+{
+  double roll;  // Rotation optimization variables
+  double pitch;
+  double yaw;
+  operator const std::string() const
+  {
+    return std::string("{") + "roll:" + std::to_string(roll) + ", pitch:" + std::to_string(pitch) +
+           ", yaw:" + std::to_string(yaw) + "}";
+  }
+  cv::Mat toMat() const
+  {
+    using cv::Mat_;
+    using std::cos;
+    using std::sin;
+
+    cv::Mat R_x = (Mat_<double>(3, 3) << 1, 0, 0, 0, cos(roll), -sin(roll), 0, sin(roll), cos(roll));
+    // Calculate rotation about y axis
+    cv::Mat R_y = (Mat_<double>(3, 3) << cos(pitch), 0, sin(pitch), 0, 1, 0, -sin(pitch), 0, cos(pitch));
+
+    // Calculate rotation about z axis
+    cv::Mat R_z = (Mat_<double>(3, 3) << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1);
+
+    return R_z * R_y * R_x;
+  }
+};
+
+cv::Mat operator*(const Rotation& lhs, const cv::Point3d& rhs);
+
+struct RotationTranslation
+{
+  Rotation rot;
+  double x;
+  double y;
+  double z;
+  operator const std::string() const
+  {
+    return std::string("{") + "roll:" + std::to_string(rot.roll) + ", pitch:" + std::to_string(rot.pitch) +
+           ", yaw:" + std::to_string(rot.yaw) + ", x:" + std::to_string(x) + ", y:" + std::to_string(y) +
+           ", z:" + std::to_string(z) + "}";
+  }
+};
+
+cv::Mat operator*(const RotationTranslation& lhs, const cv::Point3d& rhs);
+
+struct RotationCost  // equivalent to y in matlab
+{
+  double objective1;  // This is where the results of simulation is stored but not yet finalized.
+};
+
+struct RotationTranslationCost  // equivalent to y in matlab
+{
+  double objective2;  // This is where the results of simulation is stored but not yet finalized.
+};
+
+struct OptimisationSample
+{
+  cv::Point3d camera_centre{ 0, 0, 0 };
+  cv::Point3d camera_normal{ 0, 0, 0 };
+  std::vector<cv::Point3d> camera_corners;
+  cv::Point3d lidar_centre{ 0, 0, 0 };
+  cv::Point3d lidar_normal{ 0, 0, 0 };
+  std::vector<cv::Point3d> lidar_corners;
+};
+
+typedef EA::Genetic<Rotation, RotationCost> GA_Rot_t;