Commit 317573bd authored by James Ward's avatar James Ward
Browse files

Build FeatureExtractor and Optimiser as library

Facilitates testing
parent 7a2ac925
......@@ -61,25 +61,28 @@ catkin_package(
message_runtime
)
add_library(load_params src/load_params.cpp)
target_link_libraries(load_params ${catkin_LIBRARIES} ${OpenCV_LIBS})
add_executable(feature_extraction
add_library(cam_lidar_calibration
src/cam_lidar_panel.cpp
src/feature_extractor.cpp
src/load_params.cpp
src/optimiser.cpp
)
target_link_libraries(feature_extraction load_params ${catkin_LIBRARIES} ${OpenCV_LIBS} ${Eigen_LIBRARIES})
)
target_link_libraries(cam_lidar_calibration
${catkin_LIBRARIES}
${OpenCV_LIBS}
${Eigen_LIBRARIES}
Qt5::Widgets
)
add_library(cam_lidar_calibration
src/cam_lidar_panel.cpp)
target_link_libraries(cam_lidar_calibration Qt5::Widgets ${catkin_LIBRARIES})
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})
## Mark the nodelet library for installations
install(TARGETS
load_params
feature_extraction
feature_extraction_node
cam_lidar_calibration
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
......@@ -105,3 +108,10 @@ install(DIRECTORY cfg/
install(DIRECTORY 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()
#include <nodelet/nodelet.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 <std_msgs/Int8.h>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/persistence.hpp>
#include <pcl/point_cloud.h>
#include <pcl/ModelCoefficients.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"
#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;
......@@ -26,10 +29,10 @@ namespace cam_lidar_calibration
{
geometry_msgs::Quaternion normalToQuaternion(const cv::Point3d& normal);
class FeatureExtractor : public nodelet::Nodelet
class FeatureExtractor
{
public:
FeatureExtractor() = default;
FeatureExtractor();
~FeatureExtractor() = default;
void extractRegionOfInterest(const sensor_msgs::Image::ConstPtr& img,
......@@ -39,14 +42,7 @@ public:
void visualiseSamples();
void bypassInit()
{
this->onInit();
}
private:
virtual void onInit();
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);
......@@ -57,7 +53,7 @@ private:
std::pair<pcl::ModelCoefficients, pcl::ModelCoefficients>
findEdges(const pcl::PointCloud<pcl::PointXYZIR>::Ptr& edge_pair_cloud);
Optimiser optimiser_;
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_;
......
<launch>
<!--
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="feature_extraction" args="load extrinsic_calibration/feature_extraction nodelet_manager" output="screen" required="true">
</node>
-->
<rosparam file="$(find cam_lidar_calibration)/cfg/params.yaml" />
<node pkg="cam_lidar_calibration" type="feature_extraction" name="feature_extraction" output="screen">
<node pkg="cam_lidar_calibration" type="feature_extraction_node" name="feature_extraction" output="screen">
<remap from="~image" to="/gmsl/A2/image_color" />
<remap from="~pointcloud" to="/velodyne/front/points" />
</node>
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find cam_lidar_calibration)/rviz/cam_lidar_calibration.rviz" />
<!--
<node pkg="rqt_reconfigure" type="rqt_reconfigure" name="rqt_reconfigure" />
-->
</launch>
......
#include "cam_lidar_calibration/feature_extractor.h"
int main(int argc, char** argv)
{
// Initialize Node and handles
ros::init(argc, argv, "FeatureExtractor");
ros::NodeHandle n;
cam_lidar_calibration::FeatureExtractor feature_extractor;
ros::Rate loop_rate(10);
while (ros::ok())
{
feature_extractor.visualiseSamples();
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
#include "cam_lidar_calibration/feature_extractor.h"
#include <ros/ros.h>
#include "cam_lidar_calibration/point_xyzir.h"
......@@ -22,8 +24,6 @@
#include <sensor_msgs/image_encodings.h>
#include <visualization_msgs/MarkerArray.h>
#include "cam_lidar_calibration/feature_extractor.h"
using cv::findChessboardCorners;
using cv::Mat_;
using cv::Size;
......@@ -31,34 +31,16 @@ using cv::TermCriteria;
using PointCloud = pcl::PointCloud<pcl::PointXYZIR>;
int main(int argc, char** argv)
{
// Initialize Node and handles
ros::init(argc, argv, "FeatureExtractor");
ros::NodeHandle n;
cam_lidar_calibration::FeatureExtractor feature_extractor;
feature_extractor.bypassInit();
ros::Rate loop_rate(10);
while (ros::ok())
{
feature_extractor.visualiseSamples();
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
namespace cam_lidar_calibration
{
void FeatureExtractor::onInit()
FeatureExtractor::FeatureExtractor()
{
// Creating ROS nodehandle
ros::NodeHandle private_nh = ros::NodeHandle("~");
ros::NodeHandle public_nh = ros::NodeHandle();
ros::NodeHandle pnh = ros::NodeHandle("~"); // getMTPrivateNodeHandle();
loadParams(public_nh, i_params);
optimiser_ = std::make_shared<Optimiser>(i_params);
ROS_INFO("Input parameters loaded");
it_.reset(new image_transport::ImageTransport(public_nh));
......@@ -82,7 +64,7 @@ void FeatureExtractor::onInit()
optimise_service_ = public_nh.advertiseService("optimiser", &FeatureExtractor::serviceCB, this);
samples_pub_ = private_nh.advertise<visualization_msgs::MarkerArray>("collected_samples", 0);
image_publisher = it_->advertise("camera_features", 1);
NODELET_INFO_STREAM("Camera Lidar Calibration");
ROS_INFO_STREAM("Camera Lidar Calibration");
}
bool FeatureExtractor::serviceCB(Optimise::Request& req, Optimise::Response& res)
......@@ -94,14 +76,17 @@ bool FeatureExtractor::serviceCB(Optimise::Request& req, Optimise::Response& res
break;
case Optimise::Request::DISCARD:
ROS_INFO("Discarding last sample");
if (!optimiser_.samples_.empty())
if (!optimiser_->samples_.empty())
{
optimiser_.samples_.pop_back();
optimiser_->samples_.pop_back();
}
break;
case Optimise::Request::OPTIMISE:
ROS_INFO("Calling optimiser");
optimiser_.optimise();
if (!optimiser_->samples_.empty())
{
ROS_INFO("Calling optimiser");
optimiser_->optimise();
}
break;
}
......@@ -139,7 +124,7 @@ void FeatureExtractor::visualiseSamples()
visualization_msgs::Marker clear;
clear.action = visualization_msgs::Marker::DELETEALL;
vis_array.markers.push_back(clear);
for (auto& sample : optimiser_.samples_)
for (auto& sample : optimiser_->samples_)
{
visualization_msgs::Marker lidar_board, lidar_normal;
......@@ -174,9 +159,9 @@ void FeatureExtractor::visualiseSamples()
for (auto& c : sample.lidar_corners)
{
geometry_msgs::Point p;
p.x = c.x;
p.y = c.y;
p.z = c.z;
p.x = c.x / 1000;
p.y = c.y / 1000;
p.z = c.z / 1000;
lidar_board.points.push_back(p);
}
lidar_board.points.push_back(lidar_board.points[0]);
......@@ -310,7 +295,7 @@ FeatureExtractor::locateChessboard(const sensor_msgs::Image::ConstPtr& image)
cv::Mat rmat;
cv::Rodrigues(rvec, rmat);
cv::Mat z = cv::Mat(cv::Point3d(0., 0., 1.));
cv::Mat z = cv::Mat(cv::Point3d(0., 0., -1.));
auto chessboard_normal = rmat * z;
std::vector<cv::Point3d> corner_vectors;
......@@ -510,20 +495,21 @@ void FeatureExtractor::extractRegionOfInterest(const sensor_msgs::Image::ConstPt
pcl::lineWithLineIntersection(top_right, bottom_right, corner);
cv::Point3d right(corner[0], corner[1], corner[2]);
// Add points in same order as the paper
sample.lidar_corners.push_back(right);
sample.lidar_corners.push_back(top);
sample.lidar_corners.push_back(left);
sample.lidar_corners.push_back(bottom);
// Convert to mm
sample.lidar_corners.push_back(right * 1000);
sample.lidar_corners.push_back(top * 1000);
sample.lidar_corners.push_back(left * 1000);
sample.lidar_corners.push_back(bottom * 1000);
for (const auto& p : sample.lidar_corners)
{
// Average the corners, and convert to mm
sample.lidar_centre.x += p.x / 4.0 * 1000.;
sample.lidar_centre.y += p.y / 4.0 * 1000.;
sample.lidar_centre.z += p.z / 4.0 * 1000.;
// Average the corners
sample.lidar_centre.x += p.x / 4.0;
sample.lidar_centre.y += p.y / 4.0;
sample.lidar_centre.z += p.z / 4.0;
}
// Push this sample to the optimiser
optimiser_.samples_.push_back(sample);
optimiser_->samples_.push_back(sample);
} // if (flag == Optimise::Request::CAPTURE)
} // End of extractRegionOfInterest
......
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