Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
find_cam
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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment