Created
October 15, 2019 22:17
-
-
Save soulslicer/ca86a8690598519f2dab9b10e4ea649c to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment