Classes | |
struct | CalibrationInformation |
class | CameraPoseCalibrationNode |
class | CameraPoseCalibrationNodelet |
Typedefs | |
typedef std::pair < sensor_msgs::Image::ConstPtr, sensor_msgs::PointCloud2::ConstPtr > | InputData |
Functions | |
void | eraseIndices (std::vector< size_t > indices, pcl::PointCloud< pcl::PointXYZ > &cloud) |
Eigen::Isometry3d | findCalibration (cv::Mat const &image, pcl::PointCloud< pcl::PointXYZ >::Ptr cloud, cv::Size const pattern_size, double pattern_distance, double neighbor_distance=0.01, double valid_pattern_ratio_threshold=0.7, double point_cloud_scale_x=1.0, double point_cloud_scale_y=1.0, CalibrationInformation *debug_information=NULL) |
Finds the isometry for the asymmetric calibration pattern in the image and pointcloud. | |
Eigen::Isometry3d | findCalibration (cv::Mat const &left_image, cv::Mat const &right_image, Eigen::Matrix4d const &reprojection, cv::Size const pattern_size, double pattern_distance) |
Finds the isometry for the asymmetric calibration pattern, given two (undistorted) stereo images and a reprojection matrix. | |
Eigen::Isometry3d | findIsometry (pcl::PointCloud< pcl::PointXYZ >::Ptr source, pcl::PointCloud< pcl::PointXYZ >::Ptr target) |
Finds the isometry between two point clouds. | |
std::vector< size_t > | findNan (pcl::PointCloud< pcl::PointXYZ > const &cloud) |
Returns the indices with nan value for x, y or z. | |
pcl::ModelCoefficients::Ptr | fitPointsToPlane (pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud) |
Returns the plane parameters of the fit to the input point cloud. | |
pcl::PointCloud< pcl::PointXYZ > ::Ptr | generateAsymmetricCircles (double distance, size_t pattern_height, size_t pattern_width) |
Defines the asymmetric circles calibration pattern. | |
void | projectCloudOnPlane (pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud, pcl::PointCloud< pcl::PointXYZ >::Ptr cloud_projected, pcl::ModelCoefficients::ConstPtr plane_coefficients) |
Fits a plane to a point cloud and gives the projected point cloud and the plane coefficients. | |
void | synchronizationCallback (boost::promise< InputData > &promise, sensor_msgs::Image::ConstPtr image, sensor_msgs::PointCloud2::ConstPtr cloud) |
TEST (CameraPoseCalibration, findIsometry_identity) | |
Test if the isometry between two equal point clouds returns the identity transformation. | |
TEST (CameraPoseCalibration, findIsometry_translation) | |
Test if the isometry between two translated point clouds returns the translation transformation. |
typedef std::pair<sensor_msgs::Image::ConstPtr, sensor_msgs::PointCloud2::ConstPtr> camera_pose_calibration::InputData |
void camera_pose_calibration::eraseIndices | ( | std::vector< size_t > | indices, |
pcl::PointCloud< pcl::PointXYZ > & | cloud | ||
) |
indices | Indices to erase |
cloud | Point cloud to erase indices from |
Definition at line 98 of file src/camera_pose_calibration.cpp.
Eigen::Isometry3d camera_pose_calibration::findCalibration | ( | cv::Mat const & | image, |
pcl::PointCloud< pcl::PointXYZ >::Ptr | cloud, | ||
cv::Size const | pattern_size, | ||
double | pattern_distance, | ||
double | neighbor_distance = 0.01 , |
||
double | valid_pattern_ratio_threshold = 0.7 , |
||
double | point_cloud_scale_x = 1.0 , |
||
double | point_cloud_scale_y = 1.0 , |
||
CalibrationInformation * | debug_information = NULL |
||
) |
Finds the isometry for the asymmetric calibration pattern in the image and pointcloud.
image | The image in which the calibration pattern is visible. |
cloud | The corresponding (registered) pointcloud. |
pattern_size | Size of the asymmetric calibration pattern. |
pattern_distance | Distance between the center of the points on the pattern. |
neighbor_distance | Distance in meters to detected pattern points to which neighbors are searched and averaged. A distance of <= 0 means use no neighbors. |
valid_pattern_ratio_threshold | When the ratio of valid_points / (pattern_width * pattern_height) is below this threshold, there are not enough points to estimate an isometry. |
point_cloud_scale_x | Scale in x used to transform points from intensity to pointcloud frame. |
point_cloud_scale_y | Scale in y used to transform points from intensity to pointcloud frame. |
debug_information | Extra debugging information. |
Definition at line 115 of file src/camera_pose_calibration.cpp.
Eigen::Isometry3d camera_pose_calibration::findCalibration | ( | cv::Mat const & | left_image, |
cv::Mat const & | right_image, | ||
Eigen::Matrix4d const & | reprojection, | ||
cv::Size const | pattern_size, | ||
double | pattern_distance | ||
) |
Finds the isometry for the asymmetric calibration pattern, given two (undistorted) stereo images and a reprojection matrix.
left_image | The (undistorted) left image in which the calibration pattern is visible. |
right_image | The (undistorted) right image in which the calibration pattern is visible. |
reprojection | The reprojection matrix. |
pattern_size | Size of the asymmetric calibration pattern. |
pattern_distance | Distance between the center of the points on the pattern. |
Definition at line 212 of file src/camera_pose_calibration.cpp.
Eigen::Isometry3d camera_pose_calibration::findIsometry | ( | pcl::PointCloud< pcl::PointXYZ >::Ptr | source, |
pcl::PointCloud< pcl::PointXYZ >::Ptr | target | ||
) |
Finds the isometry between two point clouds.
source | Source point cloud |
target | Target point cloud |
Definition at line 106 of file src/camera_pose_calibration.cpp.
std::vector< size_t > camera_pose_calibration::findNan | ( | pcl::PointCloud< pcl::PointXYZ > const & | cloud | ) |
Returns the indices with nan value for x, y or z.
cloud | Point cloud |
Definition at line 55 of file src/camera_pose_calibration.cpp.
pcl::ModelCoefficients::Ptr camera_pose_calibration::fitPointsToPlane | ( | pcl::PointCloud< pcl::PointXYZ >::ConstPtr | cloud | ) |
Returns the plane parameters of the fit to the input point cloud.
cloud | Point cloud |
Definition at line 30 of file src/camera_pose_calibration.cpp.
pcl::PointCloud< pcl::PointXYZ >::Ptr camera_pose_calibration::generateAsymmetricCircles | ( | double | distance, |
size_t | pattern_height, | ||
size_t | pattern_width | ||
) |
Defines the asymmetric circles calibration pattern.
distance | Distance between adjacent circles |
pattern_height | Number of circles in vertical direction |
pattern_width | Number of circles in horizontal direction |
Definition at line 67 of file src/camera_pose_calibration.cpp.
void camera_pose_calibration::projectCloudOnPlane | ( | pcl::PointCloud< pcl::PointXYZ >::ConstPtr | cloud, |
pcl::PointCloud< pcl::PointXYZ >::Ptr | cloud_projected, | ||
pcl::ModelCoefficients::ConstPtr | plane_coefficients | ||
) |
Fits a plane to a point cloud and gives the projected point cloud and the plane coefficients.
Erases the indices from cloud
cloud | Point cloud |
cloud_projected | Point cloud projected to the fitted plane |
plane_coefficients | Plane coefficients |
Definition at line 86 of file src/camera_pose_calibration.cpp.
void camera_pose_calibration::synchronizationCallback | ( | boost::promise< InputData > & | promise, |
sensor_msgs::Image::ConstPtr | image, | ||
sensor_msgs::PointCloud2::ConstPtr | cloud | ||
) |
camera_pose_calibration::TEST | ( | CameraPoseCalibration | , |
findIsometry_identity | |||
) |
Test if the isometry between two equal point clouds returns the identity transformation.
Definition at line 28 of file test/camera_pose_calibration.cpp.
camera_pose_calibration::TEST | ( | CameraPoseCalibration | , |
findIsometry_translation | |||
) |
Test if the isometry between two translated point clouds returns the translation transformation.
Definition at line 47 of file test/camera_pose_calibration.cpp.