-
Notifications
You must be signed in to change notification settings - Fork 110
/
checkerboard_finder.h
79 lines (62 loc) · 2.32 KB
/
checkerboard_finder.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
/*
* Copyright (C) 2018-2023 Michael Ferguson
* Copyright (C) 2015 Fetch Robotics Inc.
* Copyright (C) 2013-2014 Unbounded Robotics Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
// Author: Michael Ferguson
#ifndef ROBOT_CALIBRATION_CAPTURE_CHECKERBOARD_FINDER_H
#define ROBOT_CALIBRATION_CAPTURE_CHECKERBOARD_FINDER_H
#include <ros/ros.h>
#include <robot_calibration/capture/depth_camera.h>
#include <robot_calibration/plugins/feature_finder.h>
#include <robot_calibration_msgs/CalibrationData.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <cv_bridge/cv_bridge.h>
namespace robot_calibration
{
/**
* \brief Finds checkerboards in images or point clouds
*/
template <typename T>
class CheckerboardFinder : public FeatureFinder
{
public:
CheckerboardFinder();
bool init(const std::string& name, ros::NodeHandle & n);
bool find(robot_calibration_msgs::CalibrationData * msg);
private:
bool findInternal(robot_calibration_msgs::CalibrationData * msg);
bool findCheckerboardPoints(const sensor_msgs::ImagePtr& msg,
std::vector<cv::Point2f>& points);
void cameraCallback(const T& msg);
bool waitForMsg();
ros::Subscriber subscriber_; /// Incoming message
ros::Publisher publisher_; /// Outgoing sensor_msgs::PointCloud2
bool waiting_;
T msg_;
DepthCameraInfoManager depth_camera_manager_;
/*
* ROS Parameters
*/
int points_x_; /// Size of checkerboard
int points_y_; /// Size of checkerboard
double square_size_; /// Size of a square on checkboard (in meters)
bool output_debug_; /// Should we output debug image/cloud?
std::string frame_id_; /// Name of checkerboard frame
std::string camera_sensor_name_;
std::string chain_sensor_name_;
};
} // namespace robot_calibration
#endif // ROBOT_CALIBRATION_CAPTURE_CHECKERBOARD_FINDER_H