Boolean find_best_corresponding_pads_or_pins(ElementPadPinData* ppd_a, int len_a, int index1_a, int index2_a, Boolean reflect, ElementPadPinData* ppd_b, int len_b, int* index1_b_ptr, int* index2_b_ptr) { int block1_start = 0; int block1_end = 0; int block2_start = 0; int block2_end = 0; if (! find_number_block(ppd_b, len_b, &ppd_a[index1_a].pp, &block1_start, &block1_end)) { return False; } if (! find_number_block(ppd_b, len_b, &ppd_a[index2_a].pp, &block2_start, &block2_end)) { return False; } Boolean min_set = False; double min_sum_of_distances = 0; int min_index1_b = 0; int min_index2_b = 0; int i; for (i = block1_start; i < block1_end; i++) { int j; for (j = block2_start; j < block2_end; j++) { /* Block1 and block2 ranges may coincide. */ if (i != j) { int k; for (k = 0; k < 4; k++) { /* Find transformation */ double angle = k * M_PI / 2; Matrix3x3 trans; if (calculate_transformation(ppd_a[index1_a].center, ppd_a[index2_a].center, ppd_b[i].center, ppd_b[j].center, reflect, angle, trans)) { /* Transform ppd_a */ transform_pad_pin_data(ppd_a, len_a, trans); /* Sum of distances */ double sd = calculate_sum_of_distances(ppd_a, len_a, ppd_b, len_b); if (! min_set || sd < min_sum_of_distances) { min_set = True; min_sum_of_distances = sd; min_index1_b = i; min_index2_b = j; } } } } } } if (min_set) { debug_log("Best corresponding pads/pins found.\n"); *index1_b_ptr = min_index1_b; *index2_b_ptr = min_index2_b; } else { debug_log("No best corresponding pads/pins found.\n"); } return min_set; }
bool send2(brio_assembly_vision_new::TrasformStampedRequest &req, brio_assembly_vision_new::TrasformStampedResponse &res) { request_a_new_cluster =true; if(find_cloud_from_kinect_head==true && final ==false) { brio_assembly_vision_new::Container * cont = new brio_assembly_vision_new::Container(); while((cont=client_call())==NULL) { std::this_thread::sleep_for (std::chrono::seconds(1)); //wait for a good response } if(first_time==true) { initial_cluster_size=cont->date_container.size(); } first_time=false; if(cont->date_container.size()>0) //after moving objects size is decreased { int i=0; //start with the lowest index std::string object_with_shape_requested = model[model_step]; while(cont->date_container[i].piece_type!=object_with_shape_requested && i<cont->date_container.size()-1) // { i++; } //end while when cluster_vector[i] has the requested shape or when the search index is bigger then cluster_vector if(i<=cont->date_container.size()-1) // if i<=cluster_vector.size() then we did find requested object at indices i { Eigen::Vector3d z_axe,x_or_y_axe; Eigen::Vector4d translation; z_axe=estimate_plane_normals(cloud); int i_center = cont->date_container[i].center_index; int i_head = cont->date_container[i].head_conn_index; x_or_y_axe= obj_axe_2_points_next(i_head,i_center); pcl::PointXYZRGB center = cloud->at(cont->date_container[i].center_index); translation[0] = center.x; translation[1] = center.y; translation[2] = center.z; calculate_transformation(x_or_y_axe,z_axe ,translation); ROS_INFO("Send TransformedPose"); Eigen::Quaternionf quat; quat = createQuaternion(); res.msg.child_frame_id = "brio_piece_frame"; res.msg.header.frame_id = "head_mount_kinect_rgb_optical_frame"; res.msg.transform.translation.x = transformata_finala(0,3); res.msg.transform.translation.y = transformata_finala(1,3); res.msg.transform.translation.z = transformata_finala(2,3); res.msg.transform.rotation.w = (double)quat.w(); res.msg.transform.rotation.x = (double)quat.x(); res.msg.transform.rotation.y = (double)quat.y(); res.msg.transform.rotation.z = (double)quat.z(); if(model_step<initial_cluster_size-1) model_step++; else { final=true; //return false; } return true; }