GenericFrame convert_disparity_message_to_generic_frame(carmen_simple_stereo_disparity_message* message)
{
Image< PixRGB<byte> > rgbimg(Dims(image_width, image_height), ZEROS);
get_depth_map_from_disparity_map(message->disparity, temp_depth_map, camera_instance, 50.0);
for(int i = 0; i < (image_width * image_height); i++)
{
temp_disparity_map[i] = message->disparity[i];
if(i > (image_width * 170) && i < ( image_width * (image_height - 110)) && temp_depth_map[i] <= 12000)
{
//temp_depth_map[i] = 12000.0 - temp_depth_map[i];
rgbimg[i].p[0] = message->reference_image[3 * i];
rgbimg[i].p[1] = message->reference_image[3 * i + 1];
rgbimg[i].p[2] = message->reference_image[3 * i + 2];
}
else
{
temp_depth_map[i] = 0.0;
rgbimg[i].p[0] = message->reference_image[3 * i] = 0.0;
rgbimg[i].p[1] = message->reference_image[3 * i + 1] = 0.0;
rgbimg[i].p[2] = message->reference_image[3 * i + 2] = 0.0;
}
}
Image<unsigned short> depthimg(temp_depth_map, Dims(image_width, image_height));
GenericFrame gf(rgbimg, depthimg);
return gf;
}
// ######################################################################
void IpcInputFrameSeries::
onSimEventClockTick(SimEventQueue& q, rutz::shared_ptr<SimEventClockTick>& e)
{
static int first_sim_event_clock = 1;
if(first_sim_event_clock)
{
Image< PixRGB<byte> > rgbimg(Dims(image_width, image_height), ZEROS);
GenericFrame gf(rgbimg);
rutz::shared_ptr<SimEventInputFrame>
ev(new SimEventInputFrame(this, gf, 0));
q.post(ev);
first_sim_event_clock = 0;
}
if(has_new_frame)
{
has_new_frame = 0;
if (frame.initialized())
{
rutz::shared_ptr<SimEventInputFrame>
ev(new SimEventInputFrame(this, frame, frame_counter));
q.post(ev);
}
}
}
void xyzrgb2xyzsift::compute() {
CLOG(LTRACE) << "xyzrgb2xyzsift::compute";
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = in_cloud_xyzrgb.read();
cloud->is_dense = true ;
cv::Mat rgbimg(cloud->height, cloud->width, CV_8UC3, cv::Scalar::all(0)) ;
for (register int row = 0; row < cloud->height; row++)
for (register int col = 0; col < cloud->width ; col++) {
pcl::PointXYZRGB& pt = cloud->at(col, row) ;
cv::Vec3b &rgbvec = rgbimg.at<cv::Vec3b>(row, col) ;
rgbvec[2] = pt.r ; //Creating image for all pixels: normal and NaN
rgbvec[1] = pt.g ;
rgbvec[0] = pt.b ;
}
std::vector<cv::KeyPoint> keypoints;
cv::Mat descriptors;
try {
//-- Step 1: Detect the keypoints.
cv::SiftFeatureDetector detector;
detector.detect(rgbimg, keypoints);
//-- Step 2: Calculate descriptors (feature vectors).
cv::SiftDescriptorExtractor extractor;
extractor.compute( rgbimg, keypoints, descriptors);
} catch (...) {
LOG(LERROR) << "sdasdas\n";
}
pcl::PointCloud<PointXYZSIFT>::Ptr cloud_xyzsift(new pcl::PointCloud<PointXYZSIFT>);
//Create output point cloud (this time - unorganized)
cloud_xyzsift->height = 1 ;
cloud_xyzsift->width = keypoints.size() ;
cloud_xyzsift->is_dense = false ;
cloud_xyzsift->points.resize(cloud_xyzsift->height * cloud_xyzsift->width) ;
//Convert SIFT keypoints/descriptors into PointXYZRGBSIFT cloud
pcl::PointCloud<PointXYZSIFT>::iterator pt_iter = cloud_xyzsift->begin();
for (register int i = 0; i < keypoints.size() ; i++) {
//std::cout << " " << keypoints[i].pt.x << " " << keypoints[i].pt.y << std::endl ;
int keyx = std::min(std::max(((unsigned int) round(keypoints[i].pt.x)), 0u), cloud->width) ;
int keyy = std::min(std::max(((unsigned int) round(keypoints[i].pt.y)), 0u), cloud->height) ;
//std::cout << keyx << " " << keyy << std::endl ;
cv::Mat desc = descriptors(cv::Range(i,i + 1), cv::Range::all()) ;
//std::cout << "descriptor " << desc << std::endl ;
//Make XYZSIFT point
PointXYZSIFT& pt_out = *pt_iter++ ;
pcl::PointXYZRGB& pt_in = cloud->at(keyx, keyy) ;
pt_out.x = pt_in.x; pt_out.y = pt_in.y; pt_out.z = pt_in.z ;
for(int j=0; j<descriptors.cols;j++){
pt_out.descriptor[j] = descriptors.row(i).at<float>(j);
}
}
out_cloud_xyzsift.write(cloud_xyzsift);
}
