00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "camera_pose_calibration_impl.hpp"
00018 #include "camera_pose_calibration.hpp"
00019
00020 #include <pcl/segmentation/sac_segmentation.h>
00021 #include <pcl/kdtree/kdtree_flann.h>
00022 #include <pcl/sample_consensus/method_types.h>
00023 #include <pcl/sample_consensus/model_types.h>
00024 #include <pcl/segmentation/sac_segmentation.h>
00025 #include <pcl/registration/transformation_estimation_svd.h>
00026 #include <pcl/filters/project_inliers.h>
00027
00028 namespace camera_pose_calibration {
00029
00030 pcl::ModelCoefficients::Ptr fitPointsToPlane(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud) {
00031 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
00032 pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
00033
00034
00035 pcl::SACSegmentation<pcl::PointXYZ> seg;
00036
00037
00038 seg.setOptimizeCoefficients(true);
00039
00040
00041 seg.setModelType(pcl::SACMODEL_PLANE);
00042 seg.setMethodType(pcl::SAC_RANSAC);
00043 seg.setDistanceThreshold(0.01);
00044
00045 seg.setInputCloud(cloud);
00046 seg.segment(*inliers, *coefficients);
00047
00048 if (inliers->indices.size() == 0) {
00049 throw std::runtime_error("Could not estimate a planar model for the given pointcloud.");
00050 }
00051
00052 return coefficients;
00053 }
00054
00055 std::vector<size_t> findNan(pcl::PointCloud<pcl::PointXYZ> const & cloud) {
00056 std::vector<size_t> nan_indices;
00057 for (size_t i = 0; i < cloud.size(); i++) {
00058 if (std::isnan(cloud.at(i).x) ||
00059 std::isnan(cloud.at(i).y) ||
00060 std::isnan(cloud.at(i).z)) {
00061 nan_indices.push_back(i);
00062 }
00063 }
00064 return nan_indices;
00065 }
00066
00067 pcl::PointCloud<pcl::PointXYZ>::Ptr generateAsymmetricCircles(
00068 double distance,
00069 size_t pattern_height,
00070 size_t pattern_width
00071 ) {
00072 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00073 for (size_t j = 0; j < pattern_height; j++) {
00074 for (size_t i = 0; i < pattern_width; i++) {
00075 pcl::PointXYZ point;
00076 double offset = (j % 2 == 0 ? 0 : distance / 2);
00077 point.x = j * 0.5 * distance;
00078 point.y = i * distance + offset;
00079 point.z = 0;
00080 cloud->push_back(point);
00081 }
00082 }
00083 return cloud;
00084 }
00085
00086 void projectCloudOnPlane(
00087 pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,
00088 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected,
00089 pcl::ModelCoefficients::ConstPtr plane_coefficients
00090 ) {
00091 pcl::ProjectInliers<pcl::PointXYZ> proj;
00092 proj.setModelType(pcl::SACMODEL_PLANE);
00093 proj.setInputCloud(cloud);
00094 proj.setModelCoefficients(plane_coefficients);
00095 proj.filter(*cloud_projected);
00096 }
00097
00098 void eraseIndices(std::vector<size_t> indices, pcl::PointCloud<pcl::PointXYZ> & cloud) {
00099
00100 std::sort(indices.begin(), indices.end(), std::greater<size_t>());
00101 for (size_t i = 0; i < indices.size(); i++) {
00102 cloud.erase(cloud.begin() + indices.at(i));
00103 }
00104 }
00105
00106 Eigen::Isometry3d findIsometry(
00107 pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target
00108 ) {
00109 Eigen::Matrix4f transformation;
00110 pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> svd;
00111 svd.estimateRigidTransformation(*source, *target, transformation);
00112 return Eigen::Isometry3d(transformation.cast<double>());
00113 }
00114
00115 Eigen::Isometry3d findCalibration(
00116 cv::Mat const & image,
00117 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
00118 cv::Size const pattern_size,
00119 double pattern_distance,
00120 double neighbor_distance,
00121 double valid_pattern_ratio_threshold,
00122 double point_cloud_scale_x,
00123 double point_cloud_scale_y,
00124 CalibrationInformation * debug_information
00125 ) {
00126 if (point_cloud_scale_x == 0) point_cloud_scale_x = 1;
00127 if (point_cloud_scale_y == 0) point_cloud_scale_y = 1;
00128
00129
00130 std::vector<cv::Point2f> image_points;
00131 if (!cv::findCirclesGrid(image, pattern_size, image_points, cv::CALIB_CB_ASYMMETRIC_GRID)) {
00132 throw std::runtime_error("Failed to find calibration pattern.");
00133 }
00134
00135 if (debug_information) {
00136 debug_information->image_points = image_points;
00137 }
00138
00139
00140 pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
00141 pcl::KdTreeFLANN<pcl::PointXYZ> kd_tree;
00142 kd_tree.setInputCloud(cloud);
00143 for (std::vector<cv::Point2f>::const_iterator i = image_points.begin(); i != image_points.end(); ++i) {
00144 cv::Point2f const & p = *i;
00145 if (p.x < 0 || p.y < 0) {
00146 throw std::runtime_error("Found invalid image point for calibration pattern point.");
00147 }
00148
00149 pcl::PointXYZ average = cloud->at(p.x * point_cloud_scale_x, p.y * point_cloud_scale_y);
00150 if (std::isnan(average.x) || std::isnan(average.y) || std::isnan(average.z)) {
00151 source_cloud->push_back(average);
00152 continue;
00153 }
00154
00155
00156 if (neighbor_distance > 0) {
00157 std::vector<int> neighbor_indices;
00158 std::vector<float> neighbor_squared_distances;
00159 kd_tree.radiusSearch(average, neighbor_distance, neighbor_indices, neighbor_squared_distances);
00160 for (std::vector<int>::const_iterator i = neighbor_indices.begin(); i != neighbor_indices.end(); ++i) {
00161 int index = *i;
00162 average.x += cloud->points[index].x;
00163 average.y += cloud->points[index].y;
00164 average.z += cloud->points[index].z;
00165 }
00166 average.x /= neighbor_indices.size() + 1;
00167 average.y /= neighbor_indices.size() + 1;
00168 average.z /= neighbor_indices.size() + 1;
00169 }
00170
00171 source_cloud->push_back(average);
00172 }
00173
00174
00175 pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud = generateAsymmetricCircles(pattern_distance, pattern_size.height, pattern_size.width);
00176
00177 if (debug_information) {
00178 debug_information->source_cloud = source_cloud;
00179 debug_information->target_cloud = target_cloud;
00180 }
00181
00182
00183 std::vector<size_t> nan_indices = findNan(*source_cloud);
00184
00185 if (double(nan_indices.size()) / pattern_size.area() > 1.f - valid_pattern_ratio_threshold) {
00186 throw std::runtime_error("Found too many invalid (NaN) points to find isometry.");
00187 }
00188
00189 eraseIndices(nan_indices, *source_cloud);
00190 eraseIndices(nan_indices, *target_cloud);
00191
00192 if (debug_information) {
00193 debug_information->nan_indices = nan_indices;
00194 }
00195
00196
00197 pcl::ModelCoefficients::Ptr plane_coefficients = fitPointsToPlane(source_cloud);
00198 pcl::PointCloud<pcl::PointXYZ>::Ptr projected_source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
00199 projectCloudOnPlane(source_cloud, projected_source_cloud, plane_coefficients);
00200
00201 if (debug_information) {
00202 debug_information->plane_coefficients = plane_coefficients;
00203 debug_information->projected_source_cloud = projected_source_cloud;
00204 }
00205
00206
00207 Eigen::Isometry3d isometry = findIsometry(projected_source_cloud, target_cloud);
00208
00209 return isometry;
00210 }
00211
00212 Eigen::Isometry3d findCalibration(
00213 cv::Mat const & left_image,
00214 cv::Mat const & right_image,
00215 Eigen::Matrix4d const & reprojection,
00216 cv::Size pattern_size,
00217 double pattern_distance
00218 ) {
00219
00220 std::vector<cv::Point2f> left_points, right_points;
00221
00222 bool detected_patterns =
00223 cv::findCirclesGrid(left_image, pattern_size, left_points, cv::CALIB_CB_ASYMMETRIC_GRID) &&
00224 cv::findCirclesGrid(right_image, pattern_size, right_points, cv::CALIB_CB_ASYMMETRIC_GRID);
00225
00226 if (!detected_patterns) {
00227 throw std::runtime_error("Failed to find calibration patterns.");
00228 }
00229
00230
00231 pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
00232 for (int i = 0; i < pattern_size.area(); ++i) {
00233 Eigen::Vector4d point(
00234 left_points[i].x, left_points[i].y, left_points[i].x - right_points[i].x, 1
00235 );
00236 Eigen::Vector3d result = (reprojection * point).hnormalized();
00237 source_cloud->push_back(pcl::PointXYZ(result[0], result[1], result[2]));
00238 }
00239
00240
00241 pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud = generateAsymmetricCircles(pattern_distance, pattern_size.height, pattern_size.width);
00242
00243
00244 pcl::ModelCoefficients::Ptr plane_coefficients = fitPointsToPlane(source_cloud);
00245 pcl::PointCloud<pcl::PointXYZ>::Ptr projected_source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
00246 projectCloudOnPlane(source_cloud, projected_source_cloud, plane_coefficients);
00247
00248
00249 Eigen::Isometry3d isometry = findIsometry(projected_source_cloud, target_cloud);
00250
00251 return isometry;
00252 }
00253
00254 }