Node::Node(ros::NodeHandle* nh,
const cv::Mat& visual, const cv::Mat& depth,
image_geometry::PinholeCameraModel cam_model,
cv::Ptr<cv::FeatureDetector> detector,
cv::Ptr<cv::DescriptorExtractor> extractor,
cv::Ptr<cv::DescriptorMatcher> matcher,
const sensor_msgs::PointCloud2ConstPtr& point_cloud,
unsigned int msg_id,
unsigned int id,
const cv::Mat& detection_mask):
nh_(nh),
msg_id_(msg_id),
id_(id),
cloudMessage_(*point_cloud),
cam_model_(cam_model),
matcher_(matcher)
{
std::clock_t starttime=std::clock();
ROS_FATAL_COND(detector.empty(), "No valid detector!");
detector->detect( visual, feature_locations_2d_, detection_mask);// fill 2d locations
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "Feature detection runtime: " << ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC );
ROS_INFO("Found %d Keypoints", (int)feature_locations_2d_.size());
cloud_pub = nh_->advertise<sensor_msgs::PointCloud2>("/rgbdslam/batch_clouds",20);
cloud_pub2 = nh_->advertise<sensor_msgs::PointCloud2>("/rgbdslam/my_clouds",20);
// get pcl::Pointcloud to extract depthValues a pixel positions
std::clock_t starttime5=std::clock();
// TODO: This takes 0.1 seconds and is not strictly necessary
//pcl::fromROSMsg(*point_cloud,pc);
pcl::fromROSMsg(*point_cloud,pc_col);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime5) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "projection runtime: " << ( std::clock() - starttime5 ) / (double)CLOCKS_PER_SEC );
// project pixels to 3dPositions and create search structures for the gicp
projectTo3D(depth, feature_locations_2d_, feature_locations_3d_,pc_col); //takes less than 0.01 sec
std::clock_t starttime4=std::clock();
// projectTo3d need a dense cloud to use the points.at(px.x,px.y)-Call
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime4) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "projection runtime: " << ( std::clock() - starttime4 ) / (double)CLOCKS_PER_SEC );
std::clock_t starttime2=std::clock();
extractor->compute(visual, feature_locations_2d_, feature_descriptors_); //fill feature_descriptors_ with information
assert(feature_locations_2d_.size() == feature_locations_3d_.size());
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime2) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "Feature extraction runtime: " << ( std::clock() - starttime2 ) / (double)CLOCKS_PER_SEC );
flannIndex = NULL;
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "constructor runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}
Node::Node(ros::NodeHandle& nh, const cv::Mat& visual,
cv::Ptr<cv::FeatureDetector> detector,
cv::Ptr<cv::DescriptorExtractor> extractor,
cv::Ptr<cv::DescriptorMatcher> matcher,
const sensor_msgs::PointCloud2ConstPtr point_cloud,
const cv::Mat& detection_mask)
: id_(0),
flannIndex(NULL),
matcher_(matcher)
{
#ifdef USE_ICP_CODE
gicp_initialized = false;
#endif
std::clock_t starttime=std::clock();
#ifdef USE_SIFT_GPU
SiftGPUFeatureDetector* siftgpu = SiftGPUFeatureDetector::GetInstance();
float* descriptors = siftgpu->detect(visual, feature_locations_2d_);
if (descriptors == NULL) {
ROS_FATAL("Can't run SiftGPU");
}
#else
ROS_FATAL_COND(detector.empty(), "No valid detector!");
detector->detect( visual, feature_locations_2d_, detection_mask);// fill 2d locations
#endif
ROS_INFO("Feature detection and descriptor extraction runtime: %f", ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "Feature detection runtime: " << ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC );
/*
if (id_ == 0)
cloud_pub_ = nh_->advertise<sensor_msgs::PointCloud2>("clouds_from_node_base",10);
else{
*/
cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/rgbdslam/batch_clouds",20);
// cloud_pub_ransac = nh_->advertise<sensor_msgs::PointCloud2>("clouds_from_node_current_ransac",10);
//} */
// get pcl::Pointcloud to extract depthValues a pixel positions
std::clock_t starttime5=std::clock();
// TODO: If batch sending/saving of clouds would be removed, the pointcloud wouldn't have to be saved
// which would slim down the Memory requirements
pcl::fromROSMsg(*point_cloud,pc_col);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime5) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "pc2->pcl conversion runtime: " << ( std::clock() - starttime5 ) / (double)CLOCKS_PER_SEC );
// project pixels to 3dPositions and create search structures for the gicp
#ifdef USE_SIFT_GPU
// removes also unused descriptors from the descriptors matrix
// build descriptor matrix
projectTo3DSiftGPU(feature_locations_2d_, feature_locations_3d_, pc_col, descriptors, feature_descriptors_); //takes less than 0.01 sec
if (descriptors != NULL) delete descriptors;
#else
projectTo3D(feature_locations_2d_, feature_locations_3d_, pc_col); //takes less than 0.01 sec
#endif
// projectTo3d need a dense cloud to use the points.at(px.x,px.y)-Call
#ifdef USE_ICP_CODE
std::clock_t starttime4=std::clock();
createGICPStructures();
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime4) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "gicp runtime: " << ( std::clock() - starttime4 ) / (double)CLOCKS_PER_SEC );
#endif
std::clock_t starttime2=std::clock();
#ifndef USE_SIFT_GPU
// ROS_INFO("Use extractor");
//cv::Mat topleft, topright;
//topleft = visual.colRange(0,visual.cols/2+50);
//topright= visual.colRange(visual.cols/2+50, visual.cols-1);
//std::vector<cv::KeyPoint> kp1, kp2;
//extractor->compute(topleft, kp1, feature_descriptors_); //fill feature_descriptors_ with information
extractor->compute(visual, feature_locations_2d_, feature_descriptors_); //fill feature_descriptors_ with information
#endif
assert(feature_locations_2d_.size() == feature_locations_3d_.size());
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime2) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "Feature extraction runtime: " << ( std::clock() - starttime2 ) / (double)CLOCKS_PER_SEC );
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "constructor runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}