Skip to content
GitLab
Commit e8bd5001 authored by James Ward's avatar James Ward
Browse files

WIP: continued refactoring of FeatureExtractor 

Make Optimiser a member of FeatureExtractor.
Clean up matrix calculations.
Separate into methods.
parent 7a176344
Expand all Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ e8bd5001
......@@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
cv_bridge
dynamic_reconfigure
image_transport
geometry_msgs
message_generation
nodelet
pcl_conversions
......@@ -38,12 +39,11 @@ set(CMAKE_AUTOMOC ON)
add_message_files(
FILES
calibration_data.msg
extrinsics.msg
)
add_service_files(
FILES
Sample.srv
Optimise.srv
)
generate_messages(
DEPENDENCIES
......@@ -64,11 +64,9 @@ catkin_package(
add_library(load_params src/load_params.cpp)
target_link_libraries(load_params ${catkin_LIBRARIES} ${OpenCV_LIBS})
add_executable(optimiser src/optimiser.cpp)
target_link_libraries(optimiser load_params ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Eigen_LIBRARIES})
add_executable(feature_extraction
src/feature_extractor.cpp
src/feature_extractor.cpp
src/optimiser.cpp
)
target_link_libraries(feature_extraction load_params ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Eigen_LIBRARIES})
......@@ -82,7 +80,6 @@ target_link_libraries(cam_lidar_calibration Qt5::Widgets ${catkin_LIBRARIES})
install(TARGETS
load_params
feature_extraction
optimiser
cam_lidar_calibration
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
......
include/cam_lidar_calibration/feature_extractor.h
View file @ e8bd5001
......@@ -13,11 +13,12 @@
#include <opencv2/core/mat.hpp>
#include <opencv2/core/persistence.hpp>
#include <cam_lidar_calibration/Sample.h>
#include <cam_lidar_calibration/Optimise.h>
#include <cam_lidar_calibration/boundsConfig.h>
#include "cam_lidar_calibration/calibration_data.h"
#include "cam_lidar_calibration/load_params.h"
#include "cam_lidar_calibration/optimiser.h"
typedef message_filters::Subscriber<sensor_msgs::Image> image_sub_type;
typedef message_filters::Subscriber<pcl::PointCloud<pcl::PointXYZIR>> pc_sub_type;
......@@ -32,7 +33,7 @@ public:
void extractRegionOfInterest(const sensor_msgs::Image::ConstPtr& img,
const pcl::PointCloud<pcl::PointXYZIR>::ConstPtr& pc);
bool sampleCB(Sample::Request& req, Sample::Response& res);
bool serviceCB(Optimise::Request& req, Optimise::Response& res);
void boundsCB(boundsConfig& config, uint32_t level);
void bypassInit()
......@@ -45,19 +46,22 @@ private:
void passthrough(const pcl::PointCloud<pcl::PointXYZIR>::ConstPtr& input_pc,
pcl::PointCloud<pcl::PointXYZIR>::Ptr& output_pc);
std::optional<std::tuple<cv::Mat, cv::Mat>> locateChessboard(const sensor_msgs::Image::ConstPtr& image);
auto chessboardProjection(const std::vector<cv::Point2f>& corners, const cv_bridge::CvImagePtr& cv_ptr);
std::optional<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);
std::optional<std::tuple<pcl::PointCloud<pcl::PointXYZIR>::Ptr, cv::Point3d>>
extractBoard(const pcl::PointCloud<pcl::PointXYZIR>::Ptr& cloud);
Optimiser optimiser_;
initial_parameters_t i_params;
std::string pkg_loc;
int cb_l, cb_b, l, b, e_l, e_b;
double diagonal;
ros::Publisher pub;
cv::FileStorage fs;
int flag = 0;
cam_lidar_calibration::boundsConfig bounds_;
cam_lidar_calibration::calibration_data sample_data;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, pcl::PointCloud<pcl::PointXYZIR>>
ImageLidarSyncPolicy;
......@@ -70,7 +74,7 @@ private:
ros::Publisher pub_cloud, expt_region;
ros::Publisher vis_pub, visPub;
image_transport::Publisher image_publisher;
ros::ServiceServer sample_service_;
ros::ServiceServer optimise_service_;
visualization_msgs::Marker marker;
boost::shared_ptr<image_transport::ImageTransport> it_;
......
include/cam_lidar_calibration/optimiser.h
View file @ e8bd5001
#ifndef optimiser_h_
#define optimiser_h_
#include "cam_lidar_calibration/openga.h"
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/PointCloud2.h>
......@@ -44,6 +46,15 @@ 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;
cv::Point3d camera_normal;
cv::Point3d lidar_centre;
cv::Point3d lidar_normal;
cv::Point3d lidar_corner;
};
typedef EA::Genetic<Rotation, RotationCost> GA_Type;
typedef EA::Genetic<RotationTranslation, RotationTranslationCost> GA_Type2;
......@@ -53,6 +64,9 @@ public:
Optimiser();
~Optimiser() = default;
bool optimise();
std::vector<OptimisationSample> samples_;
void SO_report_generation(int generation_number, const EA::GenerationType<Rotation, RotationCost>& last_generation,
const Rotation& best_genes);
double calculate_SO_total_fitness(const GA_Type::thisChromosomeType& X);
......@@ -74,11 +88,9 @@ public:
private:
double* convertToImagePoints(double x, double y, double z);
void getSamples(const cam_lidar_calibration::calibration_data::ConstPtr& data);
double rotationFitnessFunc(double e1, double e2, double e3);
void imageProjection(RotationTranslation rot_trans);
bool optimiseCB(cam_lidar_calibration::Sample::Request& req, cam_lidar_calibration::Sample::Response& res);
void sensorInfoCB(const sensor_msgs::Image::ConstPtr& img, const sensor_msgs::PointCloud2::ConstPtr& pc);
Rotation eul;
......@@ -96,7 +108,6 @@ private:
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2>
ImageLidarSyncPolicy;
std::shared_ptr<message_filters::Synchronizer<ImageLidarSyncPolicy>> sync;
ros::ServiceServer optimise_service_;
ros::Subscriber calibdata_sub_;
};
......
launch/cam_lidar_calibration.launch
View file @ e8bd5001
......@@ -12,9 +12,6 @@
<node pkg="cam_lidar_calibration" type="feature_extraction" name="feature_extraction" output="screen">
</node>
<node pkg="cam_lidar_calibration" type="optimiser" name="optimiser" output="screen">
</node>
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find cam_lidar_calibration)/rviz/cam_lidar_calibration.rviz" />
<!--
......
src/feature_extractor.cpp
View file @ e8bd5001
This diff is collapsed.
src/optimiser.cpp
View file @ e8bd5001
......@@ -17,7 +17,6 @@
#include "cam_lidar_calibration/calibration_data.h"
#include "cam_lidar_calibration/load_params.h"
#include "cam_lidar_calibration/openga.h"
#include <cam_lidar_calibration/Sample.h>
#include "cam_lidar_calibration/optimiser.h"
......@@ -396,34 +395,7 @@ void Optimiser::SO_report_generation(int generation_number,
// << eul_t.e3 << std::endl;
}
void Optimiser::getSamples(const cam_lidar_calibration::calibration_data::ConstPtr& data)
{
sample++;
ROS_INFO_STREAM("Sample " << sample);
std::vector<double> camera_p;
std::vector<double> camera_n;
std::vector<double> velodyne_p;
std::vector<double> velodyne_n;
std::vector<double> velodyne_c;
for (int i = 0; i < 3; i++)
{
velodyne_n.push_back(data->velodynenormal[i]);
velodyne_p.push_back(data->velodynepoint[i] / 1000);
camera_n.push_back(data->cameranormal[i]);
camera_p.push_back(data->camerapoint[i] / 1000);
velodyne_c.push_back(data->velodynecorner[i]);
}
calibrationdata.velodynenormals.push_back(velodyne_n);
calibrationdata.velodynepoints.push_back(velodyne_p);
calibrationdata.cameranormals.push_back(camera_n);
calibrationdata.camerapoints.push_back(camera_p);
calibrationdata.velodynecorners.push_back(velodyne_c);
calibrationdata.pixeldata.push_back(data->pixeldata);
}
bool Optimiser::optimiseCB(cam_lidar_calibration::Sample::Request& req, cam_lidar_calibration::Sample::Response& res)
bool Optimiser::optimise()
{
ROS_INFO("Input samples collected");
calibrationdata.cameranormals_mat = cv::Mat(sample, 3, CV_64F);
......@@ -745,9 +717,6 @@ Optimiser::Optimiser()
cloud = pcl::PointCloud<pcl::PointXYZIR>::Ptr(new pcl::PointCloud<pcl::PointXYZIR>);
cv_ptr = cv_bridge::CvImagePtr(new cv_bridge::CvImage);
optimise_service_ = n.advertiseService("optimise", &Optimiser::optimiseCB, this);
calibdata_sub_ = n.subscribe("roi/points", 5, &Optimiser::getSamples, this);
message_filters::Subscriber<sensor_msgs::Image> image_sub(n, i_params.camera_topic, 5);
message_filters::Subscriber<sensor_msgs::PointCloud2> pcl_sub(n, i_params.lidar_topic, 5);
sync = std::make_shared<message_filters::Synchronizer<ImageLidarSyncPolicy>>(ImageLidarSyncPolicy(5), image_sub,
......@@ -758,15 +727,3 @@ Optimiser::Optimiser()
pub_img_dist = it.advertise("image_projection", 20);
}
} // namespace cam_lidar_calibration
int main(int argc, char** argv)
{
ROS_INFO("Optimiser");
ros::init(argc, argv, "optimiser");
cam_lidar_calibration::Optimiser o;
ros::spin();
return 0;
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment