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