Skip to content

Commit

Permalink
add finder for 2d checkerboard (#155)
Browse files Browse the repository at this point in the history 
this is a backport of #152 to ROS1
  • Loading branch information
@mikeferguson
mikeferguson committed May 24, 2023
1 parent cdbb44b commit e0ab1f5
Show file tree
Hide file tree
Showing 5 changed files with 141 additions and 55 deletions.
15 changes: 9 additions & 6 deletions robot_calibration/include/robot_calibration/capture/checkerboard_finder.h
View file
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2018 Michael Ferguson
* Copyright (C) 2018-2023 Michael Ferguson
* Copyright (C) 2015 Fetch Robotics Inc.
* Copyright (C) 2013-2014 Unbounded Robotics Inc.
*
Expand Down Expand Up @@ -33,8 +33,9 @@ namespace robot_calibration
{

/**
* \brief This class processes the point cloud input to find a checkerboard
* \brief Finds checkerboards in images or point clouds
*/
template <typename T>
class CheckerboardFinder : public FeatureFinder
{
public:
Expand All @@ -44,15 +45,17 @@ class CheckerboardFinder : public FeatureFinder

private:
bool findInternal(robot_calibration_msgs::CalibrationData * msg);
bool findCheckerboardPoints(const sensor_msgs::ImagePtr& msg,
std::vector<cv::Point2f>& points);

void cameraCallback(const sensor_msgs::PointCloud2& cloud);
bool waitForCloud();
void cameraCallback(const T& msg);
bool waitForMsg();

ros::Subscriber subscriber_; /// Incoming sensor_msgs::PointCloud2
ros::Subscriber subscriber_; /// Incoming message
ros::Publisher publisher_; /// Outgoing sensor_msgs::PointCloud2

bool waiting_;
sensor_msgs::PointCloud2 cloud_;
T msg_;
DepthCameraInfoManager depth_camera_manager_;

/*
Expand Down
10 changes: 8 additions & 2 deletions robot_calibration/robot_calibration.xml
View file
@@ -1,9 +1,15 @@
<library path="librobot_calibration_feature_finders">

<class name="robot_calibration/CheckerboardFinder"
type="robot_calibration::CheckerboardFinder"
type="robot_calibration::CheckerboardFinder<sensor_msgs::PointCloud2>"
base_class_type="robot_calibration::FeatureFinder">
<description>Feature finder that detect checkerboards.</description>
<description>Feature finder that detect checkerboards in point clouds.</description>
</class>

<class name="robot_calibration/CheckerboardFinder2d"
type="robot_calibration::CheckerboardFinder<sensor_msgs::ImagePtr>"
base_class_type="robot_calibration::FeatureFinder">
<description>Feature finder that detect checkerboards in 2d images.</description>
</class>

<class name="robot_calibration/LedFinder"
Expand Down
164 changes: 118 additions & 46 deletions robot_calibration/src/finders/checkerboard_finder.cpp
View file
@@ -1,4 +1,5 @@
/*
* Copyright (C) 2023 Michael Ferguson
* Copyright (C) 2015 Fetch Robotics Inc.
* Copyright (C) 2013-2014 Unbounded Robotics Inc.
*
Expand All @@ -21,7 +22,8 @@
#include <robot_calibration/capture/checkerboard_finder.h>
#include <sensor_msgs/point_cloud2_iterator.h>

PLUGINLIB_EXPORT_CLASS(robot_calibration::CheckerboardFinder, robot_calibration::FeatureFinder)
PLUGINLIB_EXPORT_CLASS(robot_calibration::CheckerboardFinder<sensor_msgs::PointCloud2>, robot_calibration::FeatureFinder)
PLUGINLIB_EXPORT_CLASS(robot_calibration::CheckerboardFinder<sensor_msgs::ImagePtr>, robot_calibration::FeatureFinder)

namespace robot_calibration
{
Expand All @@ -31,13 +33,15 @@ const unsigned X = 0;
const unsigned Y = 1;
const unsigned Z = 2;

CheckerboardFinder::CheckerboardFinder() :
template <typename T>
CheckerboardFinder<T>::CheckerboardFinder() :
waiting_(false)
{
}

bool CheckerboardFinder::init(const std::string& name,
ros::NodeHandle & nh)
template <typename T>
bool CheckerboardFinder<T>::init(const std::string& name,
ros::NodeHandle & nh)
{
if (!FeatureFinder::init(name, nh))
return false;
Expand Down Expand Up @@ -82,17 +86,19 @@ bool CheckerboardFinder::init(const std::string& name,
return true;
}

void CheckerboardFinder::cameraCallback(const sensor_msgs::PointCloud2& cloud)
template <typename T>
void CheckerboardFinder<T>::cameraCallback(const T& msg)
{
if (waiting_)
{
cloud_ = cloud;
msg_ = msg;
waiting_ = false;
}
}

// Returns true if we got a message, false if we timeout
bool CheckerboardFinder::waitForCloud()
template <typename T>
bool CheckerboardFinder<T>::waitForMsg()
{
// Initial wait cycle so that camera is definitely up to date.
ros::Duration(1/10.0).sleep();
Expand All @@ -109,11 +115,12 @@ bool CheckerboardFinder::waitForCloud()
ros::Duration(0.01).sleep();
ros::spinOnce();
}
ROS_ERROR("Failed to get cloud");
ROS_ERROR("Failed to get message");
return !waiting_;
}

bool CheckerboardFinder::find(robot_calibration_msgs::CalibrationData * msg)
template <typename T>
bool CheckerboardFinder<T>::find(robot_calibration_msgs::CalibrationData * msg)
{
// Try up to 50 frames
for (int i = 0; i < 50; ++i)
Expand All @@ -129,35 +136,33 @@ bool CheckerboardFinder::find(robot_calibration_msgs::CalibrationData * msg)
return false;
}

bool CheckerboardFinder::findInternal(robot_calibration_msgs::CalibrationData * msg)
template <>
bool CheckerboardFinder<sensor_msgs::PointCloud2>::findInternal(robot_calibration_msgs::CalibrationData * msg)
{
geometry_msgs::PointStamped rgbd;
geometry_msgs::PointStamped world;

// Get cloud
if (!waitForCloud())
if (!waitForMsg())
{
ROS_ERROR("No point cloud data");
return false;
}

if (cloud_.height == 1)
if (msg_.height == 1)
{
ROS_ERROR("OpenCV does not support unorganized cloud/image.");
return false;
}

// Get an image message from point cloud
sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
sensor_msgs::PointCloud2ConstIterator<uint8_t> rgb(cloud_, "rgb");
sensor_msgs::PointCloud2ConstIterator<uint8_t> rgb(msg_, "rgb");
image_msg->encoding = "bgr8";
image_msg->height = cloud_.height;
image_msg->width = cloud_.width;
image_msg->height = msg_.height;
image_msg->width = msg_.width;
image_msg->step = image_msg->width * sizeof (uint8_t) * 3;
image_msg->data.resize(image_msg->step * image_msg->height);
for (size_t y = 0; y < cloud_.height; y++)
for (size_t y = 0; y < msg_.height; y++)
{
for (size_t x = 0; x < cloud_.width; x++)
for (size_t x = 0; x < msg_.width; x++)
{
uint8_t* pixel = &(image_msg->data[y * image_msg->step + x * 3]);
pixel[0] = rgb[0];
Expand All @@ -167,26 +172,8 @@ bool CheckerboardFinder::findInternal(robot_calibration_msgs::CalibrationData *
}
}

// Get an OpenCV image from the cloud
cv_bridge::CvImagePtr bridge;
try
{
bridge = cv_bridge::toCvCopy(image_msg, "mono8"); // TODO: was rgb8? does this work?
}
catch(cv_bridge::Exception& e)
{
ROS_ERROR("Conversion failed");
return false;
}

// Find checkerboard
std::vector<cv::Point2f> points;
points.resize(points_x_ * points_y_);
cv::Size checkerboard_size(points_x_, points_y_);
int found = cv::findChessboardCorners(bridge->image, checkerboard_size,
points, cv::CALIB_CB_ADAPTIVE_THRESH);

if (found)
if (findCheckerboardPoints(image_msg, points))
{
ROS_INFO("Found the checkboard");

Expand All @@ -195,7 +182,7 @@ bool CheckerboardFinder::findInternal(robot_calibration_msgs::CalibrationData *
cloud.width = 0;
cloud.height = 0;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = cloud_.header.frame_id;
cloud.header.frame_id = msg_.header.frame_id;
sensor_msgs::PointCloud2Modifier cloud_mod(cloud);
cloud_mod.setPointCloud2FieldsByString(1, "xyz");
cloud_mod.resize(points_x_ * points_y_);
Expand All @@ -211,20 +198,21 @@ bool CheckerboardFinder::findInternal(robot_calibration_msgs::CalibrationData *
msg->observations[idx_cam].features.resize(points_x_ * points_y_);
msg->observations[idx_chain].features.resize(points_x_ * points_y_);


// Fill in the headers
rgbd.header = cloud_.header;
// Setup observed points
geometry_msgs::PointStamped rgbd;
rgbd.header = msg_.header;
geometry_msgs::PointStamped world;
world.header.frame_id = frame_id_;

// Fill in message
sensor_msgs::PointCloud2ConstIterator<float> xyz(cloud_, "x");
sensor_msgs::PointCloud2ConstIterator<float> xyz(msg_, "x");
for (size_t i = 0; i < points.size(); ++i)
{
world.point.x = (i % points_x_) * square_size_;
world.point.y = (i / points_x_) * square_size_;

// Get 3d point
int index = (int)(points[i].y) * cloud_.width + (int)(points[i].x);
int index = (int)(points[i].y) * msg_.width + (int)(points[i].x);
rgbd.point.x = (xyz + index)[X];
rgbd.point.y = (xyz + index)[Y];
rgbd.point.z = (xyz + index)[Z];
Expand Down Expand Up @@ -252,7 +240,7 @@ bool CheckerboardFinder::findInternal(robot_calibration_msgs::CalibrationData *
// Add debug cloud to message
if (output_debug_)
{
msg->observations[idx_cam].cloud = cloud_;
msg->observations[idx_cam].cloud = msg_;
}

// Publish results
Expand All @@ -265,4 +253,88 @@ bool CheckerboardFinder::findInternal(robot_calibration_msgs::CalibrationData *
return false;
}

template <>
bool CheckerboardFinder<sensor_msgs::ImagePtr>::findInternal(robot_calibration_msgs::CalibrationData * msg)
{
// Get image
if (!waitForMsg())
{
ROS_ERROR("No image data");
return false;
}

std::vector<cv::Point2f> points;
if (findCheckerboardPoints(msg_, points))
{
ROS_INFO("Found the checkboard");

// Set msg size
int idx_cam = msg->observations.size() + 0;
int idx_chain = msg->observations.size() + 1;
msg->observations.resize(msg->observations.size() + 2);
msg->observations[idx_cam].sensor_name = camera_sensor_name_;
msg->observations[idx_chain].sensor_name = chain_sensor_name_;

msg->observations[idx_cam].features.resize(points_x_ * points_y_);
msg->observations[idx_chain].features.resize(points_x_ * points_y_);

// Setup observed points
geometry_msgs::PointStamped rgbd;
rgbd.header = msg_->header;
geometry_msgs::PointStamped world;
world.header.frame_id = frame_id_;

// Fill in message
for (size_t i = 0; i < points.size(); ++i)
{
// Create 3d position of corner (in checkerboard_frame)
world.point.x = (i % points_x_) * square_size_;
world.point.y = (i / points_x_) * square_size_;

// Save 2d pixel
rgbd.point.x = points[i].x;
rgbd.point.y = points[i].y;
rgbd.point.z = 0.0; // No Z information

msg->observations[idx_cam].features[i] = rgbd;
msg->observations[idx_cam].ext_camera_info = depth_camera_manager_.getDepthCameraInfo();
msg->observations[idx_chain].features[i] = world;
}

// Add debug cloud to message
if (output_debug_)
{
msg->observations[idx_cam].image = *msg_;
}

// Found all points
return true;
}

return false;
}

template <typename T>
bool CheckerboardFinder<T>::findCheckerboardPoints(const sensor_msgs::ImagePtr& image,
std::vector<cv::Point2f>& points)
{
// Get an OpenCV image from the cloud
cv_bridge::CvImagePtr bridge;
try
{
bridge = cv_bridge::toCvCopy(image, "mono8"); // TODO: was rgb8? does this work?
}
catch(cv_bridge::Exception& e)
{
ROS_ERROR("Conversion failed");
return false;
}

// Find checkerboard
points.resize(points_x_ * points_y_);
cv::Size checkerboard_size(points_x_, points_y_);
return cv::findChessboardCorners(bridge->image, checkerboard_size,
points, cv::CALIB_CB_ADAPTIVE_THRESH);
}

} // namespace robot_calibration
2 changes: 1 addition & 1 deletion robot_calibration/test/feature_finder_loader_tests.cpp
View file
Expand Up @@ -25,7 +25,7 @@ TEST(FeatureFinderLoaderTests, test_feature_finder_loader)
bool result = loader.load(nh, features);

EXPECT_EQ(true, result);
EXPECT_EQ(static_cast<size_t>(2), features.size());
EXPECT_EQ(static_cast<size_t>(3), features.size());
}

int main(int argc, char** argv)
Expand Down
5 changes: 5 additions & 0 deletions robot_calibration/test/feature_finder_loader_tests.yaml
View file
Expand Up @@ -4,6 +4,11 @@ features:
topic: /head_camera/depth_registered/points
camera_sensor_name: camera
chain_sensor_name: arm
checkerboard_finder_2d:
type: robot_calibration/CheckerboardFinder2d
topic: /head_camera/depth_registered/rgb
camera_sensor_name: camera
chain_sensor_name: arm
ground_plane_finder:
type: robot_calibration/PlaneFinder
topic: /head_camera/depth_registered/points
Expand Down

0 comments on commit e0ab1f5

Please sign in to comment.