find_cam

find_cam.cpp

    static Eigen::Matrix4Xf findCameraIntersectionsOpt(const Datum& cam_data, const std::vector<int>& good_inds, const std::vector<Point2D>& pts)
    {
        // ASK Question
        // What does imgh actually refer to. Since we know the camera is rotated. Should I be using imgw
        // I have changed it to imgw

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

        // 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;
            Eigen::Vector2f intersection_pt;
            for (int j = 0; j < (good_inds.size() - 1); j++)
            {
                int p1index = good_inds[j];
                int p2index = good_inds[j+1];

                float p1angle = angles[p1index];
                float p2angle = angles[p2index];

                if(p1angle != p2angle)
                {
                    float limit = 5;
                    if(fabs(p1angle - cam_angle) > limit && fabs(p2angle - cam_angle) > limit) continue;
                }

                Eigen::Vector2f p1(pts[p1index](0), pts[p1index](1));
                Eigen::Vector2f p2(pts[p2index](0), pts[p2index](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;

                if(found_intersection){
                    //if(p1angle != p2angle)
                    //    std::cout << cam_angle << " " << p1angle << " " << p2angle << " " << p1index << " " << p2index <<  std::endl;
                    break;
                }
            }

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