Skip to content

Instantly share code, notes, and snippets.

@soulslicer
Created October 15, 2019 22:17
Embed
What would you like to do?
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