void CameraNUI::onNextImage() { short *depth = 0; char *rgb = 0; uint32_t ts; int ret; if (triggered && trigger.empty()) { return; } else { // clear all triggers while(!trigger.empty()) trigger.read(); } // retrieve color image cv::Size size; if (resolution == FREENECT_RESOLUTION_MEDIUM) size = cv::Size(640, 480); if (resolution == FREENECT_RESOLUTION_HIGH) size = cv::Size(1280, 1024); cv::Mat image_out; ret = freenect_sync_get_video_with_res((void**)&rgb, &ts, index, (int)resolution, imageType); if(imageType == FREENECT_VIDEO_RGB) { cv::Mat tmp_rgb(size, CV_8UC3, rgb); cv::cvtColor(tmp_rgb, image_out, CV_RGB2BGR); } if(imageType == FREENECT_VIDEO_IR_8BIT) { cv::Mat tmp_gray(size, CV_8UC1, rgb); image_out = tmp_gray; } if(imageType == FREENECT_VIDEO_BAYER) { cv::Mat tmp_gray(size, CV_8UC1, rgb); image_out = tmp_gray; } // // // retrieve depth image ret = freenect_sync_get_depth((void**)&depth, &ts, index, depthType); cv::Mat tmp_depth(480, 640, CV_16UC1, depth); tmp_depth.copyTo(depthFrame); // write data to output streams //outImg.write(cameraFrame); outImg.write(image_out); outDepthMap.write(depthFrame); camera_info.write(m_camera_info); }
void CameraNUI::onNextImage() { short *depth = 0; char *rgb = 0; uint32_t ts; int ret; // retrieve color image ret = freenect_sync_get_video((void**)&rgb, &ts, 0, FREENECT_VIDEO_RGB); cv::Mat tmp_rgb(480, 640, CV_8UC3, rgb); cv::cvtColor(tmp_rgb, cameraFrame, CV_RGB2BGR); // retrieve depth image ret = freenect_sync_get_depth((void**)&depth, &ts, 0, FREENECT_DEPTH_REGISTERED); cv::Mat tmp_depth(480, 640, CV_16SC1, depth); tmp_depth.copyTo(depthFrame); // write data to output streams outImg.write(cameraFrame); outDepthMap.write(depthFrame); camera_info.write(m_camera_info); }
//*********************************************************** // segmentation // セグメンテーションを行う. //*********************************************************** void segmentation(pcl::PointCloud<pcl::PointXYZRGB>& cloud, double th, int c) { std::cout << "segmentation" << std::endl; pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_rgb (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>); for(int i=0;i<cloud.points.size();i++){ if((m_size.max_z <= cloud.points[i].z) && (cloud.points[i].z <= m_size.max_z+0.3)){ tmp_cloud->points.push_back(cloud.points[i]); } } pcl::copyPointCloud(*tmp_cloud, cloud); // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>); tree->setInputCloud (cloud.makeShared ()); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec; ec.setClusterTolerance (th); ec.setMinClusterSize (200); ec.setMaxClusterSize (300000); ec.setSearchMethod (tree); ec.setInputCloud (cloud.makeShared ()); ec.extract (cluster_indices); std::cout << "クラスタリング開始" << std::endl; int m = 0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster_rgb (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZHSV>::Ptr cloud_cluster_hsv (new pcl::PointCloud<pcl::PointXYZHSV>); pcl::PointXYZ g; for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){ cloud_cluster_rgb->points.push_back (cloud.points[*pit]); } cloud_cluster_rgb->width = cloud_cluster_rgb->points.size (); cloud_cluster_rgb->height = 1; cloud_cluster_rgb->is_dense = true; //クラスタ重心を求める g = g_pos(*cloud_cluster_rgb); if(((g.x < m_size.min_x) || (m_size.max_x < g.x)) || ((g.y < m_size.min_y) || (m_size.max_y < g.y))){ continue; } m++; if((m_size.max_z + 0.02 < g.z) && (g.z < m_size.max_z + 0.12)){ object_map map; map.cloud_rgb = *cloud_cluster_rgb; map.g = g; map.tf = c; make_elevation(map); // RGB -> HSV pcl::PointCloudXYZRGBtoXYZHSV(*cloud_cluster_rgb, *cloud_cluster_hsv); for(int i=0;i<MAP_H;i++){ for(int j=0;j<MAP_S;j++){ map.histogram[i][j] = 0.000; } } map.cloud_hsv = *cloud_cluster_hsv; // H-Sヒストグラムの作成 for(int i=0;i<cloud_cluster_hsv->points.size();i++){ if(((int)(255*cloud_cluster_hsv->points[i].h)/((256/MAP_H)*360) >= 32) || ((int)(255*cloud_cluster_hsv->points[i].h)/((256/MAP_H)*360) < 0)) continue; if(((int)(255*cloud_cluster_hsv->points[i].s)/(256/MAP_H) >= 32) || ((int)(255*cloud_cluster_hsv->points[i].s)/(256/MAP_H) < 0)) continue; // 正規化のため,セグメントの点数で割る. map.histogram[(int)(255*cloud_cluster_hsv->points[i].h)/((256/MAP_H)*360)][(int)(255*cloud_cluster_hsv->points[i].s)/(256/MAP_H)] += 1.000/(float)cloud_cluster_hsv->points.size(); } Object_Map.push_back(map); *tmp_rgb += *cloud_cluster_rgb; } } pcl::copyPointCloud(*tmp_rgb, cloud); return; }