Commit 1563f3b6 authored by Jason Webb's avatar Jason Webb
Browse files

Merge branch 'refactor' into 'master'

Refactor

See merge request !1
parents 705e9725 8b1d2f42
---
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'
}
...
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()
# Goal
---
# Result
geometry_msgs/Transform transform
---
# Feedback
/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
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 <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
#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 "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);
}
This diff is collapsed.
#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;