asr_descriptor_surface_based_recognition: pose

Go to the documentation of this file.
00001 
00021 #include "descriptor_surface_based_recognition.h"
00022 #include <ros/console.h>
00023 #include <ros/ros.h>
00024 #include <pcl/filters/filter.h>
00025 #include <pcl/filters/passthrough.h>
00026 #include <pcl_conversions/pcl_conversions.h>
00027 #include <asr_halcon_bridge/halcon_pointcloud.h>
00028 
00029 #include <pcl/common/common.h>
00030 #include <ros/package.h>
00031 
00032 #include "pose_recognition.h"
00033 #include "util.h"
00034 
00035 
00036 namespace descriptor_surface_based_recognition {
00037 
00038 PoseRecognition::PoseRecognition(HalconCpp::HImage &scene_image, HalconCpp::HImage &scene_image_mono, pcl::PointCloud<pcl::PointXYZ>::Ptr &point_cloud_with_guppy, ObjectDescriptor *obj_desc, int median_points_offset, double samplingDistance, double keypointFraction, bool eval, int max_instances)
00039     : scene_image_(scene_image), scene_image_mono_(scene_image_mono),
00040       point_cloud_with_guppy_(point_cloud_with_guppy), obj_desc_(obj_desc), max_instances_(max_instances), median_points_offset_(median_points_offset),
00041       sampling_distance_(samplingDistance), keypoint_fraction_(keypointFraction), eval_(eval) { }
00042 
00043 
00044 PoseRecognition::~PoseRecognition()
00045 {
00046     for (unsigned int i = 0; i < results_.size(); i++) {
00047         delete results_.at(i);
00048     }
00049 }
00050 
00051 void PoseRecognition::findPoses() {
00052     std::ofstream object_ofstream;
00053     std::string path = PACKAGE_PATH + "/" + OUTPUT_EVALUATION_DIR + "/" + obj_desc_->getName() + "_times.txt";
00054     if (eval_) {
00055         object_ofstream.open (path.c_str(), std::ofstream::out | std::ofstream::app);
00056     }
00057     ros::Time find_poses_start = ros::Time::now();
00058 
00059     //find object in 2D
00060     findTexture();
00061 
00062     ros::Time textures_found = ros::Time::now();
00063     if (eval_) {
00064         object_ofstream << textures_found - find_poses_start;
00065     }
00066     for (unsigned int i = 0; i < results_.size(); i++) {
00067         if (eval_) {
00068                 object_ofstream << "#";
00069         }
00070         ros::Time loop_entry = ros::Time::now();
00071 
00072         //reduce cloud
00073         if (reducePointCloud(i)) {
00074             ros::Time cloud_reduced = ros::Time::now();
00075             if (eval_) {
00076                 object_ofstream << cloud_reduced - loop_entry << ";";
00077             }
00078 
00079             //find object
00080             if (findModelInCloud(i)) {
00081                 ros::Time model_found = ros::Time::now();
00082                 if (eval_) {
00083                     object_ofstream << model_found - cloud_reduced << ";";
00084                 }
00085 
00086                 if (sqrt(pow(results_.at(i)->getPose()[0] - results_.at(i)->getTexPoint3D()[0], 2) + pow(results_.at(i)->getPose()[1] - results_.at(i)->getTexPoint3D()[1], 2) + pow(results_.at(i)->getPose()[2] - results_.at(i)->getTexPoint3D()[2], 2)) < (obj_desc_->getDiameter() * 0.5)) {
00087                     results_.at(i)->setModelFound();
00088                     //find rotation
00089                     adjustRotation(i);
00090                     if (eval_) {
00091                         object_ofstream << ros::Time::now() - model_found;
00092                     }
00093 
00094                 }
00095             }
00096         }
00097     }
00098     if (eval_) {
00099         object_ofstream << std::endl;
00100         object_ofstream.close();
00101     }
00102 }
00103 
00104 
00105 void PoseRecognition::findTexture()
00106 {
00107     for (unsigned int i = 0; i < obj_desc_->getModelViewDescriptors().size(); i++) {
00108         HalconCpp::HTuple score;
00109         if (obj_desc_->getModelViewDescriptors().at(i).getUseColor()) {
00110             scene_image_.FindUncalibDescriptorModel(obj_desc_->getModelViewDescriptors().at(i).getDescModel(), HalconCpp::HTuple(), HalconCpp::HTuple(), HalconCpp::HTuple(), HalconCpp::HTuple(), obj_desc_->getModelViewDescriptors().at(i).getScore2D(), max_instances_, "inlier_ratio", &score);
00111         } else {
00112             scene_image_mono_.FindUncalibDescriptorModel(obj_desc_->getModelViewDescriptors().at(i).getDescModel(), HalconCpp::HTuple(), HalconCpp::HTuple(), HalconCpp::HTuple(), HalconCpp::HTuple(), obj_desc_->getModelViewDescriptors().at(i).getScore2D(), max_instances_, "inlier_ratio", &score);
00113         }
00114         if (score.Length() > 0) {
00115             for (int j = 0; j < score.Length(); j++) {
00116                 HalconCpp::HTuple matrix_tuple = obj_desc_->getModelViewDescriptors().at(i).getDescModel().GetDescriptorModelResults(j, "homography");
00117                 HalconCpp::HHomMat2D matrix;
00118                 matrix.SetFromTuple(matrix_tuple);
00119 
00120                 double trans_x, trans_y, trans_w;
00121                 trans_x = matrix.ProjectiveTransPoint2d(obj_desc_->getModelViewDescriptors().at(i).getVerticalTexOffset(), obj_desc_->getModelViewDescriptors().at(i).getHorizontalTexOffset(), 1,  &trans_y, &trans_w);
00122 
00123                 int row = (int) (trans_x / trans_w);
00124                 int column = (int) (trans_y / trans_w);
00125 
00126                 results_.push_back(new RecognitionResult(i, (double)score[j], matrix, Eigen::Vector2i(row, column)));
00127                 ROS_DEBUG_STREAM("Texture of " << obj_desc_->getName() << " (instance " << j << ") found at (" << row << ", " << column << ")");
00128                 if ((int)(results_.size()) >= max_instances_) {
00129                     return;
00130                 }
00131             }
00132         }
00133     }
00134 }
00135 
00136 bool PoseRecognition::reducePointCloud(int result_index)
00137 {
00138     double radius = obj_desc_->getDiameter() / 2;
00139     int image_height = scene_image_.Height();
00140     int image_width = scene_image_.Width();
00141 
00142     pcl::PointXYZ min;
00143     pcl::PointXYZ max;
00144     pcl::getMinMax3D(*point_cloud_with_guppy_, min, max);
00145 
00146     int row = results_.at(result_index)->getTexPoint()[0];
00147     int column = results_.at(result_index)->getTexPoint()[1];
00148 
00149     std::vector<pcl::PointXYZ> points;
00150     int offsets[10] = {0, 0, median_points_offset_, 0, 0, -1 * median_points_offset_, -1 * median_points_offset_, 0, 0, median_points_offset_ };
00151 
00152     for (int i = 0; i < 5; i++) {
00153         if (row - median_points_offset_ < 0 || column - median_points_offset_ < 0 || row + median_points_offset_ >= image_height || column + median_points_offset_ >= image_width) {
00154             return false;
00155         }
00156         pcl::PointXYZ current_point = findPoint3D(point_cloud_with_guppy_, (*point_cloud_with_guppy_)(column + offsets[i * 2], row + offsets[i * 2 + 1]), row + offsets[i * 2 + 1], column + offsets[i * 2], image_height, image_width);
00157         if (pcl::isFinite(current_point)) {
00158             points.push_back(current_point);
00159         }
00160     }
00161 
00162     if (points.size() > 0) {
00163         pcl::PointXYZ median_point = computeMedian(points);
00164 
00165         if (pcl::isFinite(median_point)) {
00166             if (median_point.x - radius > min.x) { min.x = median_point.x - radius; }
00167             if (median_point.x + radius < max.x) { max.x = median_point.x + radius; }
00168             if (median_point.y - radius > min.y) { min.y = median_point.y - radius; }
00169             if (median_point.y + radius < max.y) { max.y = median_point.y + radius; }
00170             if (median_point.z - radius > min.z) { min.z = median_point.z - radius; }
00171             if (median_point.z + radius < max.z) { max.z = median_point.z + radius; }
00172 
00173         } else {
00174             return false;
00175         }
00176 
00177 
00178         pcl::PointCloud<pcl::PointXYZ>::Ptr reduced_cloud(new pcl::PointCloud<pcl::PointXYZ>);
00179 
00180         pcl::PassThrough<pcl::PointXYZ> pass;
00181         pass.setInputCloud (point_cloud_with_guppy_);
00182         pass.setFilterFieldName ("x");
00183         pass.setFilterLimits (min.x, max.x);
00184         pass.filter (*reduced_cloud);
00185 
00186         pass.setInputCloud (reduced_cloud);
00187         pass.setFilterFieldName ("y");
00188         pass.setFilterLimits (min.y, max.y);
00189         pass.filter (*reduced_cloud);
00190 
00191         results_.at(result_index)->setTexPoint3D(median_point);
00192         results_.at(result_index)->setSearchCloud(reduced_cloud);
00193 
00194         return true;
00195     }
00196     return false;
00197 
00198 }
00199 
00200 bool PoseRecognition::findModelInCloud(int result_index)
00201 {
00202 
00203     if ((results_.at(result_index)->getSearchCloud()->width * results_.at(result_index)->getSearchCloud()->height > 0)) {
00204 
00205         std::vector< int > index;
00206         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = results_.at(result_index)->getSearchCloud();
00207         pcl::removeNaNFromPointCloud(*cloud, *cloud, index);
00208 
00209         results_.at(result_index)->setSearchCloud(cloud);
00210         sensor_msgs::PointCloud2 p_n_cloud;
00211         pcl::toROSMsg(*cloud, p_n_cloud);
00212         halcon_bridge::HalconPointcloudPtr ptr = halcon_bridge::toHalconCopy(p_n_cloud);
00213         HalconCpp::HObjectModel3D scene = *ptr->model;
00214 
00215         HalconCpp::HSurfaceMatchingResultArray result;
00216         HalconCpp::HTuple *score = new HalconCpp::HTuple();
00217         HalconCpp::HTuple genParamName("num_matches");
00218         genParamName.Append("pose_ref_use_scene_normals");
00219         HalconCpp::HTuple genParamValue(1);
00220         genParamValue.Append("false");
00221         HalconCpp::HPoseArray poses = scene.FindSurfaceModel(obj_desc_->getSurfaceModel(), sampling_distance_, keypoint_fraction_, obj_desc_->getScore3D(), "false", genParamName, genParamValue, score, &result);
00222 
00223         if ((score->Length() > 0) && ((*score)[0] >= obj_desc_->getScore3D())) {
00224             bool pose_already_found = false;
00225             for (int i = 0; i < result_index; i++) {
00226                 if ((results_.at(i)->checkModelFound()) && (sqrt(pow(results_.at(i)->getPose()[0] - poses.ConvertToTuple()[0], 2) + pow(results_.at(i)->getPose()[1] - poses.ConvertToTuple()[1], 2) + pow(results_.at(i)->getPose()[2] - poses.ConvertToTuple()[2], 2)) < (obj_desc_->getDiameter() * 0.5))) {
00227                     pose_already_found = true;
00228                     break;
00229                 }
00230             }
00231             if (!pose_already_found) {
00232                 ROS_DEBUG_STREAM("Found object " << obj_desc_->getName() << " (instance " << result_index << ") in the point cloud");
00233                 results_.at(result_index)->setPose(poses.ConvertToTuple());
00234                 results_.at(result_index)->setScore3D((*score)[0]);
00235                 return true;
00236             } else {
00237                 ROS_DEBUG_STREAM("Found pose of object " << obj_desc_->getName() << " (instance " << result_index << ") was found for another instance already");
00238             }
00239         }
00240     } else {
00241         ROS_DEBUG_STREAM("Reduced point cloud of object " << obj_desc_->getName() << " (instance " << result_index << ") is empty");
00242     }
00243     return false;
00244 }
00245 
00246 void PoseRecognition::adjustRotation(int result_index)
00247 {
00248     HalconCpp::HPose pose;
00249     pose.SetFromTuple(results_.at(result_index)->getPose());
00250 
00251     HalconCpp::HQuaternion rotation;
00252     rotation.PoseToQuat(pose);
00253     rotation = rotation.QuatNormalize();
00254 
00255     Eigen::Vector3d location(results_.at(result_index)->getPose()[0], results_.at(result_index)->getPose()[1], results_.at(result_index)->getPose()[2]);
00256 
00257     //case: model is rotated around given axes
00258     switch(obj_desc_->getRotationModelType()) {
00259     case ObjectDescriptor::ROTATION_MODEL_SPHERE:
00260     {
00261         //not implemented
00262     }
00263     case ObjectDescriptor::ROTATION_MODEL_CYLINDER:
00264     {
00265         Eigen::Vector3d texFromObjOrigin;
00266         texFromObjOrigin = results_.at(result_index)->getTexPoint3D() - location;
00267         texFromObjOrigin.normalize();
00268 
00269         double x, y, z;
00270         x = rotation.QuatRotatePoint3d(obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getViewOrientation()[0], obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getViewOrientation()[1], obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getViewOrientation()[2], &y, &z);
00271         Eigen::Vector3d viewOrientationFromObjOrigin(x, y, z);
00272         viewOrientationFromObjOrigin.normalize();
00273 
00274         double dot = texFromObjOrigin.dot(viewOrientationFromObjOrigin);
00275         double maxDot = dot;
00276         double bestAngle = 0.0;
00277         double currentAngle = obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getAxes().at(0).getAngle();
00278         while (currentAngle < 360) {
00279             double radians = (currentAngle) * (PI / 180);
00280             HalconCpp::HQuaternion current_rot(obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getAxes().at(0).getAxis()[0], obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getAxes().at(0).getAxis()[1], obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getAxes().at(0).getAxis()[2], radians);
00281 
00282             double x_curr, y_curr, z_curr;
00283             x_curr = current_rot.QuatRotatePoint3d(obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getViewOrientation()[0], obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getViewOrientation()[1], obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getViewOrientation()[2], &y_curr, &z_curr);
00284 
00285             Eigen::Vector3d vec_curr_obj(x_curr, y_curr, z_curr);
00286             vec_curr_obj.normalize();
00287             x_curr = rotation.QuatRotatePoint3d(vec_curr_obj[0], vec_curr_obj[1], vec_curr_obj[2], &y_curr, &z_curr);
00288 
00289             Eigen::Vector3d vec_curr(x_curr, y_curr, z_curr);
00290             vec_curr.normalize();
00291 
00292             if (texFromObjOrigin.dot(vec_curr) > maxDot) {
00293                 maxDot = texFromObjOrigin.dot(vec_curr);
00294                 bestAngle = currentAngle;
00295             }
00296             currentAngle += obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getAxes().at(0).getAngle();
00297         }
00298         HalconCpp::HQuaternion quat_result(obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getAxes().at(0).getAxis()[0], obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getAxes().at(0).getAxis()[1], obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getAxes().at(0).getAxis()[2], bestAngle * (PI / 180));
00299         results_.at(result_index)->setAdjustedRotation(quat_result.QuatCompose(results_.at(result_index)->getAdjustedRotation()));
00300     }
00301     }
00302 
00303     //case: model is upside-down
00304     if (obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getIsInvertible()) {
00305 
00306         double trans_x, trans_y, trans_w;
00307         trans_x = results_.at(result_index)->getTransMatrix2D().ProjectiveTransPoint2d(0, 1, 1,  &trans_y, &trans_w);
00308         Eigen::Vector2d upperPoint(trans_x / trans_w, trans_y / trans_w);
00309         trans_x = results_.at(result_index)->getTransMatrix2D().ProjectiveTransPoint2d(0, 0, 1,  &trans_y, &trans_w);
00310         Eigen::Vector2d centerPoint(trans_x / trans_w, trans_y / trans_w);
00311         upperPoint = upperPoint - centerPoint;
00312         upperPoint.normalize();
00313 
00314         double x_upper, y_upper, z_upper;
00315         x_upper = rotation.QuatRotatePoint3d(0, -1, 0, &y_upper, &z_upper);
00316         Eigen::Vector2d upperPoint3D(x_upper, -1 * y_upper);
00317         upperPoint3D.normalize();
00318 
00319         if (upperPoint.dot(upperPoint3D) < 0) {
00320             double axis_rot_x, axis_rot_y, axis_rot_z;
00321             axis_rot_x = results_.at(result_index)->getAdjustedRotation().QuatRotatePoint3d(obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getViewOrientation()[0], obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getViewOrientation()[1], obj_desc_->getModelViewDescriptors().at(results_.at(result_index)->getViewIndex()).getViewOrientation()[2], &axis_rot_y, &axis_rot_z);
00322             HalconCpp::HQuaternion quat_upside_down(axis_rot_x, axis_rot_y, axis_rot_z, PI);
00323             results_.at(result_index)->setAdjustedRotation(quat_upside_down.QuatCompose(results_.at(result_index)->getAdjustedRotation()));
00324         }
00325     }
00326 }
00327 
00328 std::vector<RecognitionResult*> PoseRecognition::getResults() const { return results_; }
00329 
00330 ObjectDescriptor* PoseRecognition::getObjectDescriptor() const { return obj_desc_; }
00331 
00332 HalconCpp::HRegion PoseRecognition::getInputImageDomain() const { return scene_image_mono_.GetDomain(); }
00333 
00334 }