00001
00020 #include <rviz/mesh_loader.h>
00021 #include <rviz/ogre_helpers/render_system.h>
00022
00023 #include <OGRE/Ogre.h>
00024 #include <OGRE/OgreException.h>
00025 #include <OGRE/OgreRoot.h>
00026
00027 #include <OgreMesh.h>
00028 #include <OGRE/OgreLight.h>
00029
00030 #include <ros/package.h>
00031 #include <ros/console.h>
00032 #include <ros_uri/ros_uri.hpp>
00033
00034 #include <pcl/common/common.h>
00035 #include <pcl/point_cloud.h>
00036 #include <pcl_conversions/pcl_conversions.h>
00037 #include <pcl/point_types.h>
00038 #include <pcl/features/integral_image_normal.h>
00039 #include <pcl/common/transforms.h>
00040 #include <pcl/filters/voxel_grid.h>
00041
00042 #include <asr_halcon_bridge/halcon_image.h>
00043
00044 #include <iostream>
00045 #include <fstream>
00046
00047 #include "descriptor_surface_based_recognition.h"
00048 #include <rapidxml.hpp>
00049 #include <rapidxml_utils.hpp>
00050
00051
00052
00053
00054
00055 namespace descriptor_surface_based_recognition {
00056
00057 std::string PACKAGE_PATH;
00058
00059 DescriptorSurfaceBasedRecognition::DescriptorSurfaceBasedRecognition() : nh_(NODE_NAME), frame_counter_(0)
00060 {
00061
00062 ROS_DEBUG_STREAM("Initialize DescriptorSurfaceBasedRecognition");
00063
00064 double far_plane = 100;
00065 double near_plane = 0.01;
00066 double cx = 648.95153;
00067 double cy = 468.29311;
00068 double fx = 1689.204742;
00069 double fy = 1689.204742;
00070 double image_width = 1292.0;
00071 double image_height = 964.0;
00072 int render_width = 800;
00073 int render_height = 600;
00074 nh_.getParam("pose_val_far_plane", far_plane);
00075 nh_.getParam("pose_val_near_plane", near_plane);
00076 nh_.getParam("pose_val_cx", cx);
00077 nh_.getParam("pose_val_cy", cy);
00078 nh_.getParam("pose_val_fx", fx);
00079 nh_.getParam("pose_val_fy", fy);
00080 nh_.getParam("pose_val_image_width", image_width);
00081 nh_.getParam("pose_val_image_height", image_height);
00082 nh_.getParam("pose_val_render_image_width", render_width);
00083 nh_.getParam("pose_val_render_image_height", render_height);
00084
00085 nh_.getParam("image_color_topic", image_color_topic_);
00086 nh_.getParam("image_mono_topic", image_mono_topic_);
00087 nh_.getParam("point_cloud_topic", point_cloud_topic_);
00088 nh_.getParam("output_objects_topic", output_objects_topic_);
00089 nh_.getParam("output_marker_topic", output_marker_topic_);
00090 nh_.getParam("output_marker_bounding_box_topic", output_marker_bounding_box_topic_);
00091 nh_.getParam("output_cloud_topic", output_cloud_topic_);
00092 nh_.getParam("output_image_topic", output_image_topic_);
00093
00094 PACKAGE_PATH = ros::package::getPath("asr_descriptor_surface_based_recognition");
00095
00096
00097 reconfigure_server_.setCallback(boost::bind(&DescriptorSurfaceBasedRecognition::configCallback, this, _1, _2));
00098
00099 if (config_.evaluation) {
00100
00101 std::string path = PACKAGE_PATH + "/" + OUTPUT_EVALUATION_DIR;
00102 boost::filesystem::path dir(path.c_str());
00103 boost::filesystem::create_directory(dir);
00104 outstream_times_.open((path + "/" + OUTPUT_EVALUATION_FILE_TIME).c_str(), std::ofstream::out | std::ofstream::trunc);
00105 outstream_poses_.open((path + "/" + OUTPUT_EVALUATION_FILE_POSES).c_str(), std::ofstream::out | std::ofstream::trunc);
00106 }
00107
00108 object_db_service_client_ = nh_.serviceClient<asr_object_database::ObjectMetaData>(OBJECT_DB_SERVICE_OBJECT_TYPE);
00109 object_db_meshes_service_client_ = nh_.serviceClient<asr_object_database::RecognizerListMeshes>(OBJECT_DB_SERVICE_OBJECT_MESHES);
00110
00111 ROS_DEBUG_STREAM("Database clients initialized");
00112
00113 if (config_.usePoseValidation) {
00114 pose_val_ = PoseValidation(image_width, image_height, far_plane, near_plane, cx, cy, fx, fy, render_width, render_height);
00115 initializeMeshes();
00116 }
00117
00118 ROS_DEBUG_STREAM("Pose validation initialized");
00119
00120 thread_pool_ = boost::threadpool::pool(4);
00121 msgs_marker_array_ = boost::make_shared<visualization_msgs::MarkerArray>();
00122 msgs_box_marker_array_ = boost::make_shared<visualization_msgs::MarkerArray>();
00123
00124
00125 get_recognizer_service_ = nh_.advertiseService(GET_RECOGNIZER_SERVICE_NAME, &DescriptorSurfaceBasedRecognition::processGetRecognizerRequest, this);
00126 release_recognizer_service_ = nh_.advertiseService(RELEASE_RECOGNIZER_SERVICE_NAME, &DescriptorSurfaceBasedRecognition::processReleaseRecognizerRequest, this);
00127 get_object_list_service_ = nh_.advertiseService(GET_OBJECT_LIST_SERVICE_NAME, &DescriptorSurfaceBasedRecognition::processGetObjectListRequest, this);
00128 clear_all_recognizers_service_ = nh_.advertiseService(CLEAR_ALL_RECOGNIZERS_SERVICE_NAME, &DescriptorSurfaceBasedRecognition::processClearAllRecognizersRequest, this);
00129
00130
00131 image_sub_.subscribe(nh_, image_color_topic_, 1);
00132 image_mono_sub_.subscribe(nh_, image_mono_topic_, 1);
00133 pc_with_guppy_sub_.subscribe(nh_, point_cloud_topic_, 1);
00134
00135
00136 marker_pub_ = nh_.advertise<visualization_msgs::Marker> (output_marker_topic_, 1);
00137 boxes_pub_ = nh_.advertise<visualization_msgs::MarkerArray> (output_marker_bounding_box_topic_, 1);
00138 cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2> (output_cloud_topic_, 1);
00139 objects_pub_ = nh_.advertise<asr_msgs::AsrObject> (output_objects_topic_, 1);
00140 image_pub_ = nh_.advertise<sensor_msgs::Image> (output_image_topic_, 1);
00141
00142 sync_policy_.reset(new ApproximateSync(ApproximatePolicy(20), image_sub_, image_mono_sub_, pc_with_guppy_sub_) );
00143 sync_policy_->registerCallback(boost::bind(&DescriptorSurfaceBasedRecognition::rosCallback, this, _1, _2, _3));
00144
00145 ROS_DEBUG_STREAM("ROS services and publishers initialized");
00146
00147 ROS_INFO_STREAM("TO RECOGNIZE A NEW OBJECT CALL THE FOLLOWING SERVICE: " << std::endl <<
00148 " /asr_descriptor_surface_based_recognition/get_recognizer <object_type_name> <instance_number> <use_pose_validation>" << std::endl <<
00149 " TO END RECOGNITION FOR A SPECIFIC OBJECT CALL: " << std::endl <<
00150 " /asr_descriptor_surface_based_recognition/release_recognizer <object_type_name>" << std::endl <<
00151 " TO END RECOGNITION FOR ALL OBJECTS OF A TOPIC CALL: " << std::endl <<
00152 " /asr_descriptor_surface_based_recognition/clear_all_recognizers" << std::endl <<
00153 " TO GET A LIST OF ALL OBJECTS IT IS CURRENTLY SEARCHED FOR CALL: " << std::endl <<
00154 " /asr_descriptor_surface_based_recognition/get_object_list");
00155 }
00156
00157 bool DescriptorSurfaceBasedRecognition::processGetRecognizerRequest(GetRecognizer::Request &req, GetRecognizer::Response &res) {
00158 std::string name = req.object_name;
00159 int count = req.count;
00160 if ((count < 1) || (count > 10)) {
00161 count = 1;
00162 }
00163 bool use_pose_val = req.use_pose_val;
00164
00165 startObjectRecognition(name, count, use_pose_val) ? res.success = true : res.success = false;
00166 res.object_name = name;
00167
00168 return true;
00169 }
00170
00171 bool DescriptorSurfaceBasedRecognition::processReleaseRecognizerRequest(ReleaseRecognizer::Request &req, ReleaseRecognizer::Response &res) {
00172 std::string name = req.object_name;
00173 stopObjectRecognition(name);
00174 return true;
00175 }
00176
00177 bool DescriptorSurfaceBasedRecognition::processGetObjectListRequest(GetObjectList::Request &req, GetObjectList::Response &res) {
00178 std::vector<std::string> object_names;
00179 for (std::vector<ObjectDescriptor>::iterator iter = objects_.begin(); iter != objects_.end(); ++iter) {
00180 object_names.push_back(iter->getName());
00181 }
00182 res.objects = object_names;
00183 return true;
00184 }
00185
00186 bool DescriptorSurfaceBasedRecognition::processClearAllRecognizersRequest(ClearAllRecognizers::Request &req, ClearAllRecognizers::Response &res) {
00187 objects_.clear();
00188 meshes_.clear();
00189 return true;
00190 }
00191
00192 void DescriptorSurfaceBasedRecognition::initializeMeshes() {
00193
00194 asr_object_database::RecognizerListMeshes objectMeshes;
00195 objectMeshes.request.recognizer = OBJECT_DATABASE_CATEGORY;
00196 object_db_meshes_service_client_.call(objectMeshes.request, objectMeshes.response);
00197
00198 std::vector<std::string> mesh_paths = objectMeshes.response.meshes;
00199 for (std::vector<std::string>::iterator iter = mesh_paths.begin(); iter != mesh_paths.end(); ++iter) {
00200 rviz::loadMeshFromResource(*iter);
00201 }
00202 }
00203
00204 bool DescriptorSurfaceBasedRecognition::startObjectRecognition(std::string name, int count, bool use_pose_val) {
00205 asr_object_database::ObjectMetaData objectType;
00206 objectType.request.object_type = name;
00207 objectType.request.recognizer = OBJECT_DATABASE_CATEGORY;
00208 object_db_service_client_.call(objectType.request, objectType.response);
00209 if (objectType.response.is_valid) {
00210 std::string object_name = name;
00211 std::string xml_path = ros_uri::absolute_path(objectType.response.object_folder) + "/" + name + ".xml";
00212
00213 try {
00214 rapidxml::file<> xmlFile(xml_path.c_str());
00215 rapidxml::xml_document<> doc;
00216 doc.parse<0>(xmlFile.data());
00217
00218 rapidxml::xml_node<> *first_node = doc.first_node();
00219 if (first_node) {
00220 rapidxml::xml_node<> *node_name = first_node->first_node("name");
00221 if (node_name) {
00222 object_name = node_name->value();
00223 }
00224
00225 }
00226 } catch(std::runtime_error err) {
00227 ROS_DEBUG_STREAM("Can't parse meta-xml-file (add_object -> name). error: " << err.what());
00228 } catch (rapidxml::parse_error err) {
00229 ROS_DEBUG_STREAM("Can't parse meta-xml-file (add_object -> name). error: " << err.what());
00230 }
00231
00232 bool object_in_list = false;
00233 for (unsigned int i = 0; i < objects_.size(); i++) {
00234 if (objects_.at(i).getName() == object_name) {
00235 if (objects_.at(i).getInstanceCount() != count) {
00236 objects_.at(i).setCount(count);
00237 }
00238 if (objects_.at(i).getUsePoseVal() != use_pose_val) {
00239 objects_.at(i).setUsePoseVal(use_pose_val);
00240 }
00241 object_in_list = true;
00242 break;
00243 }
00244 }
00245 if (!object_in_list) {
00246 ObjectDescriptor object(objectType.response.object_folder + "/", xml_path, count, use_pose_val);
00247 if (object.isValid()) {
00248 objects_.push_back(object);
00249 if (pose_val_.isInitialized()) {
00250 meshes_.push_back(rviz::loadMeshFromResource(object.getMesh()));
00251 }
00252 }
00253
00254 }
00255 return true;
00256 }
00257 return false;
00258 }
00259
00260
00261 void DescriptorSurfaceBasedRecognition::stopObjectRecognition(std::string name) {
00262 for (unsigned int i = 0; i < objects_.size(); i++) {
00263 if (objects_.at(i).getName() == name) {
00264 objects_.erase(objects_.begin() + i);
00265 if (pose_val_.isInitialized()) {
00266 meshes_.erase(meshes_.begin() + i);
00267 }
00268 break;
00269 }
00270 }
00271 }
00272
00273
00274
00275 void DescriptorSurfaceBasedRecognition::rosCallback(const sensor_msgs::ImageConstPtr &input_image_guppy, const sensor_msgs::ImageConstPtr& input_image_guppy_mono, const sensor_msgs::PointCloud2ConstPtr &input_point_cloud_with_guppy)
00276 {
00277 frame_counter_++;
00278
00279 if (objects_.size() > 0) {
00280 ROS_INFO_STREAM("Enter callback for new frame");
00281 ros::Time time_frame_enter = ros::Time::now();
00282
00283 pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_with_guppy_ptr (new pcl::PointCloud<pcl::PointXYZ>);
00284 pcl::fromROSMsg(*input_point_cloud_with_guppy, *point_cloud_with_guppy_ptr);
00285
00286 HalconCpp::HImage scene_image = *halcon_bridge::toHalconCopy(input_image_guppy)->image;
00287 HalconCpp::HImage scene_image_mono = *halcon_bridge::toHalconCopy(input_image_guppy_mono)->image;
00288
00289 ROS_INFO_STREAM("Search objects");
00290 std::string object_list = "[";
00291 for (unsigned int i = 0; i < objects_.size(); i++) {
00292 if (i > 0) {
00293 object_list += ", ";
00294 }
00295 object_list += objects_.at(i).getName();
00296 }
00297 object_list += "]";
00298 ROS_DEBUG_STREAM("Searching for: " << object_list);
00299
00300
00301 std::vector<PoseRecognition*> pose_recs;
00302 for (unsigned int i = 0; i < objects_.size(); i++) {
00303 HalconCpp::HImage scene_image_curr = scene_image;
00304 HalconCpp::HImage scene_image_mono_curr = scene_image_mono;
00305 for (unsigned int j = 0; j < last_frame_positions_.size(); j++) {
00306 if (last_frame_positions_.at(j).getObjectIndex() == (int)(i)) {
00307 if ((int)(last_frame_positions_.at(j).getPositions().size()) == objects_.at(i).getInstanceCount()) {
00308 HalconCpp::HRegion region;
00309 for (unsigned int k = 0; k < last_frame_positions_.at(j).getPositions().size(); k++) {
00310 if (k == 0) {
00311 region.GenRectangle1(last_frame_positions_.at(j).getPositions().at(k)[0] - last_frame_positions_.at(j).getSearchRadii().at(k), last_frame_positions_.at(j).getPositions().at(k)[1] - last_frame_positions_.at(j).getSearchRadii().at(k), last_frame_positions_.at(j).getPositions().at(k)[0] + last_frame_positions_.at(j).getSearchRadii().at(k), last_frame_positions_.at(j).getPositions().at(k)[1] + last_frame_positions_.at(j).getSearchRadii().at(k));
00312 } else {
00313 HalconCpp::HRegion region_curr;
00314 region_curr.GenRectangle1(last_frame_positions_.at(j).getPositions().at(k)[0] - last_frame_positions_.at(j).getSearchRadii().at(k), last_frame_positions_.at(j).getPositions().at(k)[1] - last_frame_positions_.at(j).getSearchRadii().at(k), last_frame_positions_.at(j).getPositions().at(k)[0] + last_frame_positions_.at(j).getSearchRadii().at(k), last_frame_positions_.at(j).getPositions().at(k)[1] + last_frame_positions_.at(j).getSearchRadii().at(k));
00315 region = region.Union2(region_curr);
00316 }
00317 }
00318 scene_image_curr = scene_image_curr.ReduceDomain(region);
00319 scene_image_mono_curr = scene_image_mono_curr.ReduceDomain(region);
00320 }
00321 break;
00322 }
00323 }
00324 pose_recs.push_back(new PoseRecognition(scene_image_curr, scene_image_mono_curr, point_cloud_with_guppy_ptr, &(objects_.at(i)), config_.medianPointsOffset, config_.samplingDistance, config_.keypointFraction, config_.evaluation, objects_.at(i).getInstanceCount()));
00325 }
00326
00327
00328
00329
00330 for (unsigned int i = 0; i < pose_recs.size(); i++) {
00331 thread_pool_.schedule(boost::bind(&DescriptorSurfaceBasedRecognition::threadTask, this, pose_recs.at(i)));
00332 }
00333 thread_pool_.wait();
00334 thread_pool_.clear();
00335 ros::Duration search_duration = ros::Time::now() - time_frame_enter;
00336
00337
00338 ros::Duration validation_duration;
00339 if (pose_val_.isInitialized()) {
00340 ros::Time validation_time_start = ros::Time::now();
00341 for (unsigned int i = 0; i < pose_recs.size(); i++) {
00342 if (objects_.at(i).getUsePoseVal()) {
00343 for (unsigned int j = 0; j < pose_recs.at(i)->getResults().size(); j++) {
00344 if (pose_recs.at(i)->getResults().at(j)->checkModelFound()) {
00345 HalconCpp::HTuple matrix_tuple = pose_val_.validateObject(&objects_.at(i), pose_recs.at(i)->getResults().at(j), meshes_.at(i));
00346 if (matrix_tuple.Length() > 0) {
00347 HalconCpp::HHomMat2D matrix;
00348 matrix.SetFromTuple(matrix_tuple);
00349 if (!(pose_val_.compareHomographyMatrices(pose_recs.at(i)->getResults().at(j)->getTransMatrix2D(), matrix, config_.poseValidationDistanceError))) {
00350 pose_recs.at(i)->getResults().at(j)->setModelFound(false);
00351 pose_recs.at(i)->getResults().at(j)->setPoseValid(false);
00352 ROS_DEBUG_STREAM("Found homography of object " << objects_.at(i).getName() << " (instance " << j << ") in the validation image does not match the homography found in the scene");
00353 }
00354 } else {
00355 ROS_DEBUG_STREAM("Object " << objects_.at(i).getName() << " (instance " << j << ") could not be found in the validation image");
00356 if (config_.aggressivePoseValidation) {
00357 ROS_DEBUG_STREAM("Object " << objects_.at(i).getName() << " (instance " << j << ") is set as invalid due to the aggressive validation strategy");
00358 pose_recs.at(i)->getResults().at(j)->setModelFound(false);
00359 pose_recs.at(i)->getResults().at(j)->setPoseValid(false);
00360 }
00361 }
00362 }
00363 }
00364 }
00365 }
00366
00367 validation_duration = ros::Time::now() - validation_time_start;
00368 }
00369
00370
00371
00372 ros::Time visualisation_time_start = ros::Time::now();
00373
00374
00375 if (config_.evaluation) {
00376 outstream_poses_ << frame_counter_ << ";";
00377 }
00378 clearMarkers();
00379 last_frame_positions_.clear();
00380 for (unsigned int i = 0; i < pose_recs.size(); i++) {
00381 std::string name = pose_recs.at(i)->getObjectDescriptor()->getName();
00382 std::string mesh_path = pose_recs.at(i)->getObjectDescriptor()->getMesh();
00383 Object2DPositions positions(i);
00384 if (config_.useVisualisationColor) {
00385 overlaySceneWith2DResults(pose_recs.at(i), &scene_image);
00386 } else {
00387 overlaySceneWith2DResults(pose_recs.at(i), &scene_image_mono);
00388 }
00389 for (unsigned int j = 0; j < pose_recs.at(i)->getResults().size(); j++) {
00390 if (pose_recs.at(i)->getResults().at(j)->checkModelFound()) {
00391 asr_msgs::AsrObjectPtr object = createAsrMessage(pose_recs.at(i), j, input_point_cloud_with_guppy->header);
00392 geometry_msgs::PoseWithCovariance obj_pose = object->sampledPoses.front();
00393 if (config_.evaluation) {
00394 HalconCpp::HQuaternion quat;
00395 HalconCpp::HTuple quat_tuple = HalconCpp::HTuple::TupleGenConst(4, 0);
00396 quat_tuple[0] = obj_pose.pose.orientation.w;
00397 quat_tuple[1] = obj_pose.pose.orientation.x;
00398 quat_tuple[2] = obj_pose.pose.orientation.y;
00399 quat_tuple[3] = obj_pose.pose.orientation.z;
00400 quat.SetFromTuple(quat_tuple);
00401
00402 double axis_x_x, axis_x_y, axis_x_z;
00403 axis_x_x = quat.QuatRotatePoint3d(1, 0, 0, &axis_x_y, &axis_x_z);
00404 double axis_y_x, axis_y_y, axis_y_z;
00405 axis_y_x = quat.QuatRotatePoint3d(0, 1, 0, &axis_y_y, &axis_y_z);
00406 double axis_z_x, axis_z_y, axis_z_z;
00407 axis_z_x = quat.QuatRotatePoint3d(0, 0, 1, &axis_z_y, &axis_z_z);
00408
00409 outstream_poses_ << object->type << "_" << object->identifier << ";" << obj_pose.pose.position.x << ";" << obj_pose.pose.position.y << ";" << obj_pose.pose.position.z << ";" << obj_pose.pose.orientation.w << ";" << obj_pose.pose.orientation.x << ";" << obj_pose.pose.orientation.y << ";" << obj_pose.pose.orientation.z << ";" << pose_recs.at(i)->getResults().at(j)->getScore2D() << ";" << pose_recs.at(i)->getResults().at(j)->getScore3D() << ";" << axis_x_x << ";" << axis_x_y << ";" << axis_x_z << ";" << axis_y_x << ";" << axis_y_y << ";" << axis_y_z << ";" << axis_z_x << ";" << axis_z_y << ";" << axis_z_z << "#";
00410 }
00411 ROS_INFO_STREAM("------- " << name << " found: " << obj_pose.pose.position.x << " " << obj_pose.pose.position.y << " " << obj_pose.pose.position.z << " " << obj_pose.pose.orientation.w << " " << obj_pose.pose.orientation.x << " " << obj_pose.pose.orientation.y << " " << obj_pose.pose.orientation.z << "; score 2D: " << pose_recs.at(i)->getResults().at(j)->getScore2D() << ", score 3D: " << pose_recs.at(i)->getResults().at(j)->getScore3D());
00412 createMarker(object, mesh_path);
00413 positions.addPosition(pose_recs.at(i)->getResults().at(j)->getTexPoint(), pose_recs.at(i)->getResults().at(j)->getSearchRadius());
00414 objects_pub_.publish(object);
00415 } else {
00416 if (pose_recs.at(i)->getResults().at(j)->getPoseValid()) {
00417 ROS_INFO_STREAM("------- " << name << " not found but texture found with score: " << pose_recs.at(i)->getResults().at(j)->getScore2D());
00418 } else {
00419 ROS_INFO_STREAM("------- " << name << " found but pose is invalid");
00420 }
00421 }
00422 }
00423 last_frame_positions_.push_back(positions);
00424 }
00425
00426
00427 pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_cloud = createVisualizationCloud(pose_recs, input_point_cloud_with_guppy->header);
00428 result_cloud->header = pcl_conversions::toPCL(input_point_cloud_with_guppy->header);
00429 sensor_msgs::PointCloud2 reduced_cloud_sensor_msg;
00430 pcl::toROSMsg(*result_cloud, reduced_cloud_sensor_msg);
00431 cloud_pub_.publish(reduced_cloud_sensor_msg);
00432
00433
00434 boxes_pub_.publish(msgs_box_marker_array_);
00435
00436
00437 halcon_bridge::HalconImagePtr ptr = boost::make_shared<halcon_bridge::HalconImage>();
00438 ptr->image = new HalconCpp::HImage();
00439 if (config_.useVisualisationColor) {
00440 ptr->header = input_image_guppy->header;
00441 ptr->encoding = input_image_guppy->encoding;
00442 *ptr->image = scene_image;
00443 } else {
00444 ptr->header = input_image_guppy_mono->header;
00445 ptr->encoding = input_image_guppy_mono->encoding;
00446 *ptr->image = scene_image_mono;
00447 }
00448 sensor_msgs::ImagePtr result = ptr->toImageMsg();
00449 image_pub_.publish(result);
00450
00451
00452 ROS_INFO_STREAM("Time for search: " << search_duration << " seconds");
00453 if (pose_val_.isInitialized()) {
00454 ROS_INFO_STREAM("Time for pose validation: " << validation_duration << " seconds");
00455 }
00456 ROS_INFO_STREAM("Time for visualisation: " << ros::Time::now() - visualisation_time_start << " seconds");
00457 ROS_INFO_STREAM("Time for frame: " << ros::Time::now() - time_frame_enter << " seconds");
00458
00459
00460 if (config_.evaluation) {
00461 outstream_times_ << frame_counter_ << "#" << search_duration << ";";
00462 if (pose_val_.isInitialized()) {
00463 outstream_times_ << validation_duration << ";";
00464 } else {
00465 outstream_times_ << "-;";
00466 }
00467 outstream_times_ << ros::Time::now() - visualisation_time_start << ";" << ros::Time::now() - time_frame_enter << ";" << std::endl;
00468 outstream_poses_ << std::endl;
00469 }
00470
00471
00472 std::cout << std::endl;
00473
00474 for (unsigned int i = 0; i < objects_.size(); i++) {
00475 delete pose_recs.at(i);
00476 }
00477 }
00478 }
00479
00480
00481 void DescriptorSurfaceBasedRecognition::configCallback(DescriptorSurfaceBasedRecognitionConfig &config, uint32_t level)
00482 {
00483 this->config_ = config;
00484 }
00485
00486
00487 void DescriptorSurfaceBasedRecognition::threadTask(PoseRecognition *pose_rec)
00488 {
00489 pose_rec->findPoses();
00490 }
00491
00492
00493 asr_msgs::AsrObjectPtr DescriptorSurfaceBasedRecognition::createAsrMessage(PoseRecognition *pose_rec, int results_index, std_msgs::Header header)
00494 {
00495 HalconCpp::HPose pose;
00496 pose.SetFromTuple(pose_rec->getResults().at(results_index)->getPose());
00497 HalconCpp::HQuaternion quaternion;
00498 quaternion.PoseToQuat(pose);
00499 quaternion = quaternion.QuatCompose(pose_rec->getResults().at(results_index)->getAdjustedRotation());
00500
00501 asr_msgs::AsrObjectPtr object;
00502 object.reset(new asr_msgs::AsrObject());
00503
00504 object->header = header;
00505 object->providedBy = "descriptor_surface_based_recognition";
00506
00507 geometry_msgs::Pose current_pose;
00508
00509 current_pose.position.x = (double)pose.ConvertToTuple()[0];
00510 current_pose.position.y = (double)pose.ConvertToTuple()[1];
00511 current_pose.position.z = (double)pose.ConvertToTuple()[2];
00512
00513 current_pose.orientation.w = (double)quaternion.ConvertToTuple().ToDArr()[0];
00514 current_pose.orientation.x = (double)quaternion.ConvertToTuple().ToDArr()[1];
00515 current_pose.orientation.y = (double)quaternion.ConvertToTuple().ToDArr()[2];
00516 current_pose.orientation.z = (double)quaternion.ConvertToTuple().ToDArr()[3];
00517
00518 geometry_msgs::PoseWithCovariance current_pose_with_c;
00519 current_pose_with_c.pose = current_pose;
00520 for(unsigned int i = 0; i < current_pose_with_c.covariance.size(); i++)
00521 current_pose_with_c.covariance.at(i) = 0.0f;
00522
00523 object->sampledPoses.push_back(current_pose_with_c);
00524
00525 HalconCpp::HTuple bounding_corners = pose_rec->getObjectDescriptor()->getSurfaceModel().GetSurfaceModelParam("bounding_box1");
00526 boost::array< ::geometry_msgs::Point_<std::allocator<void> > , 8> bounding_box;
00527 for (unsigned int z = 0; z < 2; z++) {
00528 for (unsigned int y = 0; y < 2; y++) {
00529 for (unsigned int x = 0; x < 2; x++) {
00530 bounding_box[4 * z + 2 * y + x].x = (double)bounding_corners[x * 3];
00531 bounding_box[4 * z + 2 * y + x].y = (double)bounding_corners[y * 3 + 1];
00532 bounding_box[4 * z + 2 * y + x].z = (double)bounding_corners[z * 3 + 2];
00533 }
00534 }
00535 }
00536 object->boundingBox = bounding_box;
00537
00538 object->colorName = "textured";
00539 object->type = pose_rec->getObjectDescriptor()->getName();
00540
00541 object->identifier = boost::lexical_cast<std::string>(results_index);
00542 object->meshResourcePath = pose_rec->getObjectDescriptor()->getMesh();
00543
00544
00545
00546 object->sizeConfidence = pose_rec->getResults().at(results_index)->getScore3D();
00547 object->typeConfidence = pose_rec->getResults().at(results_index)->getScore2D();
00548 return object;
00549 }
00550
00551
00552 void DescriptorSurfaceBasedRecognition::createMarker(asr_msgs::AsrObjectPtr &object, std::string &mesh_path)
00553 {
00554 visualization_msgs::Marker marker;
00555 marker.header = object->header;
00556 marker.ns = "Recognition results";
00557 marker.id = msgs_marker_array_->markers.size() + 1;
00558 marker.action = visualization_msgs::Marker::ADD;
00559 marker.pose.position = object->sampledPoses.front().pose.position;
00560
00561 marker.pose.orientation = object->sampledPoses.front().pose.orientation;
00562
00563 marker.color.a = 0;
00564 marker.color.r = 0;
00565 marker.color.g = 0;
00566 marker.color.b = 0;
00567 marker.scale.x = 0.001;
00568 marker.scale.y = 0.001;
00569 marker.scale.z = 0.001;
00570
00571 marker.mesh_use_embedded_materials = true;
00572 marker.type = visualization_msgs::Marker::MESH_RESOURCE;
00573 marker.mesh_resource = mesh_path;
00574 marker.lifetime = ros::Duration(5);
00575
00576 msgs_marker_array_->markers.push_back(marker);
00577 marker_pub_.publish(marker);
00578 }
00579
00580
00581 void DescriptorSurfaceBasedRecognition::clearMarkers()
00582 {
00583 for (unsigned int i = 0; i < msgs_marker_array_->markers.size(); i++) {
00584 msgs_marker_array_->markers[i].action = visualization_msgs::Marker::DELETE;
00585 }
00586 for (unsigned int i = 0; i < msgs_box_marker_array_->markers.size(); i++) {
00587 msgs_box_marker_array_->markers[i].action = visualization_msgs::Marker::DELETE;
00588 }
00589 msgs_marker_array_->markers.clear();
00590
00591 boxes_pub_.publish(msgs_box_marker_array_);
00592 msgs_box_marker_array_->markers.clear();
00593 }
00594
00595
00596 void DescriptorSurfaceBasedRecognition::createBoxMarker(RecognitionResult *result, std_msgs::Header header, rgb color, bool drawCompleteBoxes)
00597 {
00598 pcl::PointXYZ min;
00599 pcl::PointXYZ max;
00600 pcl::getMinMax3D(*(result->getSearchCloud()), min, max);
00601
00602 visualization_msgs::Marker marker;
00603 marker.header = header;
00604 marker.ns = "Result Bounding Boxes";
00605 marker.id = msgs_box_marker_array_->markers.size() + 1;
00606 marker.action = visualization_msgs::Marker::ADD;
00607
00608 marker.pose.orientation.w = 1.0;
00609 marker.type = visualization_msgs::Marker::LINE_LIST;
00610
00611 marker.scale.x = 0.01;
00612
00613 marker.color.r = color.r;
00614 marker.color.g = color.g;
00615 marker.color.b = color.b;
00616 marker.color.a = 1.0;
00617
00618 marker.lifetime = ros::Duration();
00619
00620 geometry_msgs::Point p;
00621
00622
00623 p.x = min.x;
00624 p.y = min.y;
00625 p.z = min.z;
00626 marker.points.push_back(p);
00627 p.x = max.x;
00628 marker.points.push_back(p);
00629
00630
00631 marker.points.push_back(p);
00632 p.y = max.y;
00633 marker.points.push_back(p);
00634
00635
00636 marker.points.push_back(p);
00637 p.x = min.x;
00638 marker.points.push_back(p);
00639
00640
00641 marker.points.push_back(p);
00642 p.y = min.y;
00643 marker.points.push_back(p);
00644
00645
00646 if (drawCompleteBoxes) {
00647
00648 p.x = min.x;
00649 p.y = min.y;
00650 p.z = max.z;
00651 marker.points.push_back(p);
00652 p.x = max.x;
00653 marker.points.push_back(p);
00654
00655
00656 marker.points.push_back(p);
00657 p.y = max.y;
00658 marker.points.push_back(p);
00659
00660
00661 marker.points.push_back(p);
00662 p.x = min.x;
00663 marker.points.push_back(p);
00664
00665
00666 marker.points.push_back(p);
00667 p.y = min.y;
00668 marker.points.push_back(p);
00669
00670
00671
00672 p.x = min.x;
00673 p.y = min.y;
00674 p.z = min.z;
00675 marker.points.push_back(p);
00676 p.z = max.z;
00677 marker.points.push_back(p);
00678
00679
00680 p.x = max.x;
00681 p.y = min.y;
00682 p.z = min.z;
00683 marker.points.push_back(p);
00684 p.z = max.z;
00685 marker.points.push_back(p);
00686
00687
00688 p.x = max.x;
00689 p.y = max.y;
00690 p.z = min.z;
00691 marker.points.push_back(p);
00692 p.z = max.z;
00693 marker.points.push_back(p);
00694
00695
00696 p.x = min.x;
00697 p.y = max.y;
00698 p.z = min.z;
00699 marker.points.push_back(p);
00700 p.z = max.z;
00701 marker.points.push_back(p);
00702 }
00703
00704 msgs_box_marker_array_->markers.push_back(marker);
00705 }
00706
00707
00708 void DescriptorSurfaceBasedRecognition::overlaySceneWith2DResults(PoseRecognition *pose_rec, HalconCpp::HImage *scene_image)
00709 {
00710 const int BOX_BORDER_SIZE = config_.boundingBoxBorderSize;
00711 const int ORIENTATION_TRIANGLE_SIZE = config_.orientationTriangleSize;
00712 const int POINTS_RADIUS = config_.featurePointsRadius;
00713
00714 HalconCpp::HTuple color_points, color_box;
00715 if (config_.useVisualisationColor) {
00716 color_points.Append(config_.visualisationColorPointsRed);
00717 color_points.Append(config_.visualisationColorPointsGreen);
00718 color_points.Append(config_.visualisationColorPointsBlue);
00719 color_box.Append(config_.visualisationColorBoxRed);
00720 color_box.Append(config_.visualisationColorBoxGreen);
00721 color_box.Append(config_.visualisationColorBoxBlue);
00722 } else {
00723 color_points.Append(255);
00724 color_box.Append(255);
00725 }
00726
00727 for (unsigned int i = 0; i < pose_rec->getResults().size(); i++) {
00728
00729
00730 HalconCpp::HTuple row, column;
00731 HalconCpp::HRegion search_points;
00732 for (unsigned int k = 0; k <= i; k++) {
00733 try {
00734 pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getDescModel().GetDescriptorModelPoints("search", k, &row, &column);
00735 search_points.GenCircle(row, column, HalconCpp::HTuple::TupleGenConst(row.Length(), POINTS_RADIUS));
00736 *scene_image = search_points.PaintRegion(*scene_image, color_points, HalconCpp::HTuple("fill"));
00737 }catch(HalconCpp::HException exc) {
00738 }
00739 }
00740
00741
00742 pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getDescModel().GetDescriptorModelPoints("model", "all", &row, &column);
00743 HalconCpp::HRegion bounding_box;
00744 bounding_box.GenRegionPoints(row, column);
00745 Hlong row1, column1, row2, column2;
00746 bounding_box.SmallestRectangle1(&row1, &column1, &row2, &column2);
00747
00748 HalconCpp::HTuple rows, columns;
00749 double temp;
00750 double upperLeftInnerX, upperRightInnerX, lowerRightInnerX, lowerLeftInnerX;
00751 double upperLeftInnerY, upperRightInnerY, lowerRightInnerY, lowerLeftInnerY;
00752 upperLeftInnerX = pose_rec->getResults().at(i)->getTransMatrix2D().ProjectiveTransPoint2d(pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(0)[1], pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(0)[0], 1.0, &upperLeftInnerY, &temp);
00753 upperLeftInnerX = upperLeftInnerX / temp;
00754 upperLeftInnerY = upperLeftInnerY / temp;
00755 rows.Append((int)upperLeftInnerX);
00756 columns.Append((int)upperLeftInnerY);
00757
00758 upperRightInnerX = pose_rec->getResults().at(i)->getTransMatrix2D().ProjectiveTransPoint2d(pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(1)[1], pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(1)[0], 1.0, &upperRightInnerY, &temp);
00759 rows.Append((int)(upperRightInnerX / temp));
00760 columns.Append((int)(upperRightInnerY / temp));
00761
00762 lowerRightInnerX = pose_rec->getResults().at(i)->getTransMatrix2D().ProjectiveTransPoint2d(pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(2)[1], pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(2)[0], 1.0, &lowerRightInnerY, &temp);
00763 rows.Append((int)(lowerRightInnerX / temp));
00764 columns.Append((int)(lowerRightInnerY / temp));
00765
00766 lowerLeftInnerX = pose_rec->getResults().at(i)->getTransMatrix2D().ProjectiveTransPoint2d(pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(3)[1], pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(3)[0], 1.0, &lowerLeftInnerY, &temp);
00767 rows.Append((int)(lowerLeftInnerX / temp));
00768 columns.Append((int)(lowerLeftInnerY / temp));
00769
00770 rows.Append((int)upperLeftInnerX);
00771 columns.Append((int)upperLeftInnerY);
00772
00773 HalconCpp::HRegion inner_box;
00774 inner_box.GenRegionPolygonFilled(rows, columns);
00775
00776 HalconCpp::HTuple rows_outer, columns_outer;
00777 double temp_outer;
00778 double upperLeftOuterX, upperRightOuterX, lowerRightOuterX, lowerLeftouterX;
00779 double upperLeftOuterY, upperRightOuterY, lowerRightOuterY, lowerLeftOuterY;
00780 upperLeftOuterX = pose_rec->getResults().at(i)->getTransMatrix2D().ProjectiveTransPoint2d(pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(0)[1] - BOX_BORDER_SIZE, pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(0)[0] - BOX_BORDER_SIZE, 1.0, &upperLeftOuterY, &temp_outer);
00781 upperLeftOuterX = upperLeftOuterX / temp_outer;
00782 upperLeftOuterY = upperLeftOuterY / temp_outer;
00783 rows_outer.Append((int)upperLeftOuterX);
00784 columns_outer.Append((int)upperLeftOuterY);
00785
00786 upperRightOuterX = pose_rec->getResults().at(i)->getTransMatrix2D().ProjectiveTransPoint2d(pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(1)[1] - BOX_BORDER_SIZE, pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(1)[0] + BOX_BORDER_SIZE, 1.0, &upperRightOuterY, &temp_outer);
00787 rows_outer.Append((int)(upperRightOuterX / temp_outer));
00788 columns_outer.Append((int)(upperRightOuterY / temp_outer));
00789
00790 lowerRightOuterX = pose_rec->getResults().at(i)->getTransMatrix2D().ProjectiveTransPoint2d(pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(2)[1] + BOX_BORDER_SIZE, pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(2)[0] + BOX_BORDER_SIZE, 1.0, &lowerRightOuterY, &temp_outer);
00791 rows_outer.Append((int)(lowerRightOuterX / temp_outer));
00792 columns_outer.Append((int)(lowerRightOuterY / temp_outer));
00793
00794 lowerLeftouterX = pose_rec->getResults().at(i)->getTransMatrix2D().ProjectiveTransPoint2d(pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(3)[1] + BOX_BORDER_SIZE, pose_rec->getObjectDescriptor()->getModelViewDescriptors().at(pose_rec->getResults().at(i)->getViewIndex()).getBoxCorners().at(3)[0] - BOX_BORDER_SIZE, 1.0, &lowerLeftOuterY, &temp_outer);
00795 rows_outer.Append((int)(lowerLeftouterX / temp_outer));
00796 columns_outer.Append((int)(lowerLeftOuterY / temp_outer));
00797
00798 rows_outer.Append((int)upperLeftOuterX);
00799 columns_outer.Append((int)upperLeftOuterY);
00800
00801 HalconCpp::HRegion outer_box;
00802 outer_box.GenRegionPolygonFilled(rows_outer, columns_outer);
00803
00804 inner_box = outer_box.Difference(inner_box);
00805
00806 *scene_image = inner_box.PaintRegion(*scene_image, color_box, HalconCpp::HTuple("fill"));
00807
00808
00809
00810 Hlong r, c, r2, c2;
00811 inner_box.SmallestRectangle1(&r, &c, &r2, &c2);
00812 int max_expansion = r2 - r;
00813 if (c2 - c > max_expansion) {
00814 max_expansion = c2 - c;
00815 }
00816 pose_rec->getResults().at(i)->setSearchRadius((int)(max_expansion * 0.5));
00817
00818
00819
00820 rows.Clear();
00821 columns.Clear();
00822 double triLeftX, triRightX, triUpX;
00823 double triLeftY, triRightY, triUpY;
00824 triLeftX = pose_rec->getResults().at(i)->getTransMatrix2D().ProjectiveTransPoint2d(ORIENTATION_TRIANGLE_SIZE, -1 * ORIENTATION_TRIANGLE_SIZE, 1.0, &triLeftY, &temp);
00825 triLeftX = triLeftX / temp;
00826 triLeftY = triLeftY / temp;
00827 rows.Append((int)triLeftX);
00828 columns.Append((int)triLeftY);
00829
00830 triRightX = pose_rec->getResults().at(i)->getTransMatrix2D().ProjectiveTransPoint2d(ORIENTATION_TRIANGLE_SIZE, ORIENTATION_TRIANGLE_SIZE, 1.0, &triRightY, &temp);
00831 rows.Append((int)(triRightX / temp));
00832 columns.Append((int)(triRightY / temp));
00833
00834 triUpX = pose_rec->getResults().at(i)->getTransMatrix2D().ProjectiveTransPoint2d(-2.5 * ORIENTATION_TRIANGLE_SIZE, 0, 1.0, &triUpY, &temp);
00835 rows.Append((int)(triUpX / temp));
00836 columns.Append((int)(triUpY / temp));
00837
00838 rows.Append((int)triLeftX);
00839 columns.Append((int)triLeftY);
00840
00841 HalconCpp::HRegion orientation_triangle;
00842 orientation_triangle.GenRegionPolygonFilled(rows, columns);
00843 *scene_image = orientation_triangle.PaintRegion(*scene_image, color_box, HalconCpp::HTuple("fill"));
00844 }
00845 }
00846
00847
00848 pcl::PointCloud<pcl::PointXYZRGB>::Ptr DescriptorSurfaceBasedRecognition::createVisualizationCloud(std::vector<PoseRecognition*> &pose_recs, std_msgs::Header header)
00849 {
00850 pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00851
00852 int num_colors = 0;
00853 for (unsigned int i = 0; i < pose_recs.size(); i++) {
00854 for(unsigned int j = 0; j < pose_recs.at(i)->getResults().size(); j++) {
00855 if (pose_recs.at(i)->getResults().at(j)->checkModelFound()) {
00856 num_colors++;
00857 }
00858 }
00859 }
00860 std::vector<rgb> colors;
00861 if (num_colors > 0) {
00862 for(int i = 0; i < 360; i += 360 / num_colors) {
00863 hsv c;
00864 c.h = i;
00865 c.s = 0.9;
00866 c.v = 0.5;
00867 colors.push_back(hsv2rgb(c));
00868 }
00869 } else {
00870 hsv c;
00871 c.h = 0;
00872 c.s = 0.9;
00873 c.v = 0.5;
00874 colors.push_back(hsv2rgb(c));
00875 }
00876 int counter = 0;
00877 for (unsigned int i = 0; i < pose_recs.size(); i++) {
00878 for(unsigned int j = 0; j < pose_recs.at(i)->getResults().size(); j++) {
00879 if (pose_recs.at(i)->getResults().at(j)->checkModelFound()) {
00880
00881
00882 if (config_.drawCloudBoxes) {
00883 createBoxMarker(pose_recs.at(i)->getResults().at(j), header, colors.at(counter), config_.drawCompleteCloudBoxes);
00884 }
00885
00886 pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
00887 pcl::copyPointCloud(*(pose_recs.at(i)->getResults().at(j)->getSearchCloud()), *tmp);
00888
00889 for(unsigned int j = 0; j < tmp->width * tmp->height; j++) {
00890 tmp->points[j].r = colors.at(counter).r * 255;
00891 tmp->points[j].g = colors.at(counter).g * 255;
00892 tmp->points[j].b = colors.at(counter).b * 255;
00893 }
00894 *result_cloud += *tmp;
00895
00896 counter = (counter + 1) % colors.size();
00897 }
00898 }
00899 }
00900
00901 if (counter == 0) {
00902
00903 pcl::PointXYZRGB point;
00904 point.x = 1000;
00905 point.y = 1000;
00906 point.z = 1000;
00907 result_cloud->push_back(point);
00908 }
00909 return result_cloud;
00910 }
00911
00912 }
00913
00914 int
00915 main (int argc, char** argv)
00916 {
00917
00918 ros::init (argc, argv, "asr_descriptor_surface_based_recognition");
00919
00920 descriptor_surface_based_recognition::DescriptorSurfaceBasedRecognition rec;
00921
00922 ros::spin();
00923
00924 return 0;
00925 }