opt.cpp

    static Eigen::Matrix4Xf findCameraIntersectionsOpt2(const Datum& cam_data, const std::vector<int>& good_inds, const std::vector<Point2D>& pts)
    {
        // Empty Matrix - Number of points of size rows
        Eigen::Matrix4Xf design_pts(4, cam_data.imgw);
        int valid_points = 0;

        // Calculate camera ray intersections for design points
        Eigen::Vector2f p0(0, 0);

        // Store angles
        float nanVal = std::numeric_limits<float>::quiet_NaN();
        std::vector<float> angles;
        angles.resize(pts.size());
        for(int i=0; i<angles.size(); i++){
            angles[i] = -((atan2f(pts[i](1), pts[i](0)) * 180 / M_PI) - 90) + 0 + 0;
        }

        // Bins
        std::vector<int> bins;
        bins.resize(cam_data.imgw+1, -1);

        for(int i=0; i<bins.size(); i++){
            const float& cam_angle = cam_data.valid_angles[i];
            float left_angle;
            float right_angle;
            if(i == 0){
                left_angle = -100;
                right_angle = cam_data.valid_angles[i];
            }else if(i == bins.size()-1){
                left_angle = cam_data.valid_angles[i-1];
                right_angle = 100;
            }else{
                left_angle = cam_data.valid_angles[i-1];
                right_angle = cam_data.valid_angles[i];
            }
            for (int j = 0; j < (good_inds.size()); j++)
            {
                const float& pt_angle = angles[good_inds[j]];
                if (pt_angle >= left_angle && pt_angle <= right_angle)  {
                    bins[i] = good_inds[j];
                    break;
                }
            }
        }

        // Iterate each column/ray of the camera
        //#pragma omp parallel for shared(cam_data, design_pts)
        for (int i = 0; i < cam_data.imgw; i++) {

            // Get Ray along mid
            Eigen::Vector2f dir(cam_data.midrays[0].at<float>(0, i), cam_data.midrays[2].at<float>(0, i));
            dir.normalize();
            Ray cam_ray = Ray(p0, dir);
            float cam_angle = cam_data.valid_angles[i];

            // Iterate the valid set of points (Need to ensure the points are continuous)
            bool found_intersection = false;
            Eigen::Vector2f intersection_pt(0.,0.);

            // Start at right bin and search for points
            int g1 = -1;
            int g2 = -1;
            for(int j=i; j>=0; j--){
                if(bins[j] == -1) continue;
                g1 = bins[j];
                break;
            }
            for(int j=i+1; j<bins.size(); j++){
                if(bins[j] == -1) continue;
                g2 = bins[j];
                break;
            }

            if(g1 != -1 && g2 != -1){
                Eigen::Vector2f p1(pts[g1](0), pts[g1](1));
                Eigen::Vector2f p2(pts[g2](0), pts[g2](1));

                // Create the intersection point for camera
                Line p1p2 = Line::Through(p1, p2);
                Eigen::Vector2f pt = cam_ray.intersectionPoint(p1p2); //guaranteed to be on line from p1 to p2

                // Check if pt is between the two design points
                // from: https://www.lucidar.me/en/mathematics/check-if-a-point-belongs-on-a-line-segment/
                Eigen::Vector2f p1p2_vec = p2 - p1;
                Eigen::Vector2f p1pt_vec = pt - p1;

                float k_p1p2 = p1p2_vec.dot(p1p2_vec); //max distance if point is between p1 and p2;
                float k_p1pt = p1p2_vec.dot(p1pt_vec);

                if (k_p1pt < 0)
                    found_intersection = false;
                else if (k_p1pt > k_p1p2)
                    found_intersection = false;
                else if (abs(k_p1pt) < FLT_EPSILON) {
                    found_intersection = true;
                    intersection_pt = pt;
                } else if (abs(k_p1pt - k_p1p2) < FLT_EPSILON) {
                    found_intersection = true;
                    intersection_pt = pt;
                } else if (k_p1pt > 0 && k_p1pt < k_p1p2) {
                    found_intersection = true;
                    intersection_pt = pt;
                }
                else
                    found_intersection = false;
            }

            float cp = (float)i/float(cam_data.imgw);
            if(cam_data.limit > 0) if(cp > cam_data.limit) found_intersection = false;
            if(cam_data.limit < 0) if(cp < fabs(cam_data.limit)) found_intersection = false;

            if (found_intersection) {
                design_pts(0, i) = intersection_pt(0); //x-value of pt
                design_pts(1, i) = 0; //y-value of pt (zero since in xz plane)
                design_pts(2, i) = intersection_pt(1); //z-value of pt
                design_pts(3, i) = 1; // 1 to make pt homogenous
                valid_points+=1;
            } else {
                design_pts(0, i) = 0; //x-value of pt
                design_pts(1, i) = 0; //y-value of pt (zero since in xz plane)
                design_pts(2, i) = 0; //z-value of pt
                design_pts(3, i) = -1; // -1 indicates bad point
            }

        }

        return design_pts;
    }