Mat TableObjectDetector::determinePlaneRotation(cv::Mat normal) {
// Graham Schmidt orthogonalization
int mind = 0; double mval = 999;
for (int i=0; i<3; i++) {
if (abs(normal.at<double>(i)) < mval) {
mind = i;
mval = abs(normal.at<double>(i));
}
}
Mat vstart = -mval*normal;
vstart.at<double>(mind) = 1;
vstart = vstart/cv::norm(vstart);
Mat v2 = normal.cross(vstart);
v2 = v2/cv::norm(v2);
Mat v3 = normal.cross(v2);
v3 = v3/cv::norm(v3);
Mat R(3, 3, CV_64F);
v2.copyTo(R.col(0));
v3.copyTo(R.col(1));
normal.copyTo(R.col(2));
return R;
}
cv::Mat findRotation(cv::Mat v1, cv::Mat v2){
cv::Mat ab = v1.cross(v2);
//std::cout << "cross " << ab << std::endl;
float s = sqrt(ab.at<float>(0,0)*ab.at<float>(0,0) + ab.at<float>(1,0)*ab.at<float>(1,0) + ab.at<float>(2,0)*ab.at<float>(2,0));
if (s == 0)
return cv::Mat::eye(3,3,CV_32F);
float c = v1.at<float>(0,0)*v2.at<float>(0,0) + v1.at<float>(1,0)*v2.at<float>(1,0) + v1.at<float>(2,0)*v2.at<float>(2,0);
cv::Mat sk = skew(ab);
return cv::Mat::eye(3,3,CV_32F) + sk + sk*sk*(1-c)/(s*s);
}
double ScaleEstimator::calcLineIntersection(cv::Mat d1, cv::Mat d2, cv::Mat p2, cv::Mat& ip1) {
cv::Mat p1(3, 1, CV_64FC1); p1 = 0.0f;
cv::Mat ret(3, 1, CV_64FC1);
double t = 0.0f;
cv::Mat normalVector = d1.cross(d2);
t = (p2 - p1).cross(d2).dot(normalVector) / cv::norm(normalVector) / cv::norm(normalVector);
ret = p1 + t*d1;
ip1 = ret.clone();
return cv::norm(normalVector.dot(p2 - p1)) / cv::norm(normalVector);
}