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;
}
Exemple #2
0
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);
}
Exemple #3
0
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);
}