// Calculates the first intersection point of the sphere and the ray with
// the direction d.
int calculateIntersectionPoint(cv::Vec3d d) {
double t1 = (- (d.t() * (e - c))[0] + sqrt(discriminant(d)))
/ (d.t() * d)[0];
double t2 = (- (d.t() * (e - c))[0] - sqrt(discriminant(d)))
/ (d.t() * d)[0];
return std::min(t1, t2);
}
void
RendererIterator::view_params(cv::Vec3d &T, cv::Vec3d &up) const
{
float angle_rad = angle_ * CV_PI / 180.;
// from http://www.xsi-blog.com/archives/115
// compute the Point(x, y ,z) on the sphere based on index_ and radius_ using Golden Spiral technique
static float inc = CV_PI * (3 - sqrt(5));
static float off = 2.0f / float(n_points_);
float y = index_ * off - 1.0f + (off / 2.0f);
float r = sqrt(1.0f - y * y);
float phi = index_ * inc;
float x = std::cos(phi) * r;
float z = std::sin(phi) * r;
float lat = std::acos(z), lon;
if ((fabs(std::sin(lat)) < 1e-5) || (fabs(y / std::sin(lat)) > 1))
lon = 0;
else
lon = std::asin(y / std::sin(lat));
x *= radius_; // * cos(lon) * sin(lat);
y *= radius_; //float y = radius * sin(lon) * sin(lat);
z *= radius_; //float z = radius * cos(lat);
T = cv::Vec3d(x, y, z);
// Figure out the up vector
float x_up = radius_ * std::cos(lon) * std::sin(lat - 1e-5) - x;
float y_up = radius_ * std::sin(lon) * std::sin(lat - 1e-5) - y;
float z_up = radius_ * std::cos(lat - 1e-5) - z;
normalize_vector(x_up, y_up, z_up);
// Figure out the third vector of the basis
float x_right = -y_up * z + z_up * y;
float y_right = x_up * z - z_up * x;
float z_right = -x_up * y + y_up * x;
normalize_vector(x_right, y_right, z_right);
// Rotate the up vector in that basis
float x_new_up = x_up * std::cos(angle_rad) + x_right * std::sin(angle_rad);
float y_new_up = y_up * std::cos(angle_rad) + y_right * std::sin(angle_rad);
float z_new_up = z_up * std::cos(angle_rad) + z_right * std::sin(angle_rad);
up = cv::Vec3d(x_new_up, y_new_up, z_new_up);
// compute the left vector
cv::Vec3d l;
l = up.cross(T); // cross product
normalize_vector(l(0),l(1),l(2));
up = T.cross(l); // cross product
normalize_vector(up(0), up(1), up(2));
}
DepthFinder::DepthFinder( const cv::Mat_<cv::Vec3f>& pc, const cv::Vec3d &focVec)
{
imgRct_ = cv::Rect( 0, 0, pc.cols, pc.rows);
// Integral image with single channel for depth
const cv::Size imgSz( imgRct_.width, imgRct_.height);
cv::Mat_<double> dmap(imgSz);
cv::Mat_<byte> dcnts(imgSz);
for ( int i = 0; i < imgSz.height; ++i)
{
double* drow = dmap.ptr<double>(i);
byte* irow = dcnts.ptr<byte>(i);
for ( int j = 0; j < imgSz.width; ++j)
{
const cv::Vec3f& fp = pc.at<cv::Vec3f>(i,j);
cv::Vec3d p( fp[0], fp[1], fp[2]);
double depth = focVec.dot( p);
if ( depth < 0)
depth = 0;
if ( depth > MAX_DEPTH)
depth = MAX_DEPTH;
drow[j] = depth;
irow[j] = depth > 0 ? 1 : 0; // Only points with valid depth are set
} // end for
} // end for
cv::integral( dmap, depthIntImg, CV_64F);
cv::integral( dcnts, depthCntImg, CV_32S);
} // end ctor
// Calculates the cosine between two cv::Vec3d vectors.
double cosVec3d(cv::Vec3d a, cv::Vec3d b) {
// Sorry, I'm afraid OpenCV in Debian repos is out of date :( There aren't
// even operator[] and operator/ for cv::Vec3d vectors.
a = a * (1 / cv::norm(a));
b = b * (1 / cv::norm(b));
return (a.t() * b)[0];
}
// public static
double ObjModelPolygonAngles::calcAngle( const cv::Vec3f& v, const cv::Vec3f& v0, const cv::Vec3f& v1)
{
const cv::Vec3d u0 = v0 - v;
const cv::Vec3d u1 = v1 - v;
return acos( u0.dot(u1) / (cv::norm(u0) * cv::norm(u1)));
} // end calcAngle
int discriminant(cv::Vec3d d) {
return pow((d.t() * (e - c))[0], 2)
- (d.t() * d)[0] * (((e - c).t() * (e - c))[0] - r * r);
}
void
set_d(float d)
{
p = cv::Vec3d(0, 0, d / n[2]);
p_dot_n = p.dot(n);
}