float ScanFeatures::calcMean(sensor_msgs::LaserScan::ConstPtr &scan_in, int points) { float dist = 0; for (int i=0; i < points; i++) { dist += scan_in.get()->ranges.at(i); } dist /= (float)points; return dist; }
void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in) { float ang_res = scan_in.get()->angle_increment; float ang_min= scan_in.get()->angle_min; std::vector<float> ranges = scan_in.get()->ranges; //////////////////////////////////////////////////////// ///Do Clustering! /// input : ranges /// output : start/end points of each cluster //////////////////////////////////////////////////////// cv::Mat V1 = (cv::Mat_<double>(4, 1) << 0, 0, 0, 1); cv::Mat U1 = (cv::Mat_<double>(3, 1) << 0, 0, 1); // Intrinsic double fx = 283.11208; double fy = 283.11208; double cx = 320; double cy = 240; cv::Mat intrinsic = (cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1 ); // Extrinsic cv::Mat extrinsic = (cv::Mat_<double>(3, 4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0); U1 = intrinsic * extrinsic * V1; U1/=U1.at<double>(2, 0); }