C++ Ptr::at方法代码示例

Eigen::Vector3d obj_axe_2_points_next(int i_head,int i_center){
    pcl::PointXYZRGB center,head;
    center = cloud->at(i_center); //x1 y1 z1
    head = cloud->at(i_head); //x2 y2 z2
    Eigen::Vector3d vector_l;
    vector_l(0)=5*(head.x - center.x);
    vector_l(1)=5*(head.y - center.y);
    vector_l(2)=5*(head.z - center.z);

    return vector_l;
}

void App::VisZ(TUMDataSetVisualizer * viewer) {
    float const maxZ = max_element(Input_->begin(), Input_->end(),
            [] (PointType const& lft, PointType const& rgh) {
                if (pcl_isfinite(lft.z) && pcl_isfinite(rgh.z)) {
                    return lft.z < rgh.z;
                } else {
                    return ! pcl_isfinite(lft.z);
                }
            })->z;
    viewer->EasyAdd(Input_, "input", [this, &maxZ] (int i) { // #6
                float const relZ = Input_->at(i).z / maxZ;
                return Color({static_cast<int>(relZ * 255), 0, static_cast<int>((1 - relZ) * 255)});
            });
}

int ObjectAspect::calculate(const cv::Mat &image, const PointCloud::Ptr &pointcloud) {

    cv::Mat gray,timg;

    image.copyTo(this->image);
    this->points = pointcloud;

#if 1
    cv::cvtColor(image,gray,CV_BGR2HLS);
    //gray = planes[2]-planes[1];
    std::vector<cv::Mat> planes;
    cv::split(gray,planes);

    gray = planes[1];
    cv::merge(&planes[0],(size_t)3,timg);
    cv::cvtColor(timg,timg,CV_HSV2BGR);
#else
    timg = image;
#endif

#ifdef DEBUG_VIS
    cv::cvtColor(gray,timg,CV_GRAY2BGR);
#endif

    const int HessianThreshold = 500;
    cv::SURF bug(HessianThreshold);
    bug.extended = false;

    bug(gray,255*cv::Mat::ones(gray.rows,gray.cols,CV_8U),keypoints,descriptors);

    for(size_t i = 0; i < keypoints.size(); i++) {
        cv::KeyPoint kp = keypoints[i];
        PointType pt3d = pointcloud->at(kp.pt.x,kp.pt.y);
        pt3d.rgb = kp.response;
        if(!isnan(pt3d.x)/*&&(kp.response/1000>3)*/) {
            keypoints3D->points.push_back(pt3d);
            map2D3D.insert(std::make_pair<int,int>(i,keypoints3D->points.size()-1));
            map2D3Dinv.insert(std::make_pair<int,int>(keypoints3D->points.size()-1,i));
        }
    }

#ifdef DEBUG_VIS
    plotKeypoints(timg);

    cv::namedWindow("surf");
    cv::imshow("surf",timg);
#endif

    return 0;
}

void App::VisOriginal(TUMDataSetVisualizer * viewer) {
    viewer->EasyAdd(Input_, "input", [this] (int i) {
                return Color(Input_->at(i));
            });
}

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;
            }