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