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

   }