int main() { scanf("%lf%lf",&r,&d); double r1=sqrt(fabs(r*r-d*d/4.0)); // printf("r1= %lf\n",r1); p1.read();p2.read(); double res=1e100; double step=2*pi/num_interval; for (int i=0;i<num_interval;++i){ res=min(res,solve(i*step,i*step+step,r1,0.5*d)); } P3 o1(0,0,0),o2(0,0,d); if (fabs((p1-o1).norm()-r)<eps && fabs((p2-o1).norm()-r)<eps){ //puts("a"); res=min(res,calc(p1,p2)); } if (fabs((p1-o2).norm()-r)<eps && fabs((p2-o2).norm()-r)<eps){ //puts("b"); res=min(res,calc(p1,p2)); } printf("%.10f\n",res); return 0; }
void GLWidget::rotateView(const QPoint& newPos) { float dx = (float)(newPos.x() - _oldMousePos.x()) / (float)_width; float dy = (float)(newPos.y() - _oldMousePos.y()) / (float)_height; _oldMousePos = newPos; updateDir(); // rotate eye around center _eye.rotate(_center, _dirx, -dx * PI ); _eye.rotate(_center, _diry, dy*PI ); // now calculate the new up-vector P3<float> upCenter = _center + _up; upCenter.rotate( _center , _diry, dy*PI); _up = upCenter - _center; _up *= 1.0/_up.norm(); updateGL(); }
void GLWidget::updateDir() { _dirx = _up; P3<float> newy = _up.cross(_center-_eye); _diry = newy * ( (float)1.0/ newy.norm() ); }
void nl_shinji_kneip_ransac(NormalAOPoseAdapter<POSE_T, POINT_T>& adapter, const POINT_T thre_3d_, const POINT_T thre_2d_, const POINT_T nl_thre, int& Iter, POINT_T confidence = 0.99){ typedef Matrix<POINT_T, Dynamic, Dynamic> MX; typedef Matrix<POINT_T, 3, 1> P3; typedef SE3Group<POSE_T> RT; POSE_T cos_thr = cos(atan(thre_2d_ / adapter.getFocal())); POINT_T cos_nl_thre = cos(nl_thre); RandomElements<int> re((int)adapter.getNumberCorrespondences()); const int K = 3; MX Xw(3, K + 1), Xc(3, K + 1), bv(3, K + 1); MX Nw(3, K + 1), Nc(3, K + 1); Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(), 3); adapter.setMaxVotes(-1); for (int ii = 0; ii < Iter; ii++) { //randomly select K candidates RT solution_kneip, solution_shinji, solution_nl; vector<RT> v_solutions; vector<int> selected_cols; re.run(K + 1, &selected_cols); if (assign_sample<POSE_T, POINT_T>(adapter, selected_cols, &Xw, &Nw, &Xc, &Nc, &bv)){ solution_shinji = shinji<POSE_T, POINT_T>(Xw, Xc, K); v_solutions.push_back(solution_shinji); } if (kneip<POSE_T, POINT_T>(Xw, bv, &solution_kneip)){ v_solutions.push_back(solution_kneip); } nl_2p<POSE_T, POINT_T>(Xc.col(0), Nc.col(0), Xc.col(1), Xw.col(0), Nw.col(0), Xw.col(1), &solution_nl); v_solutions.push_back(solution_nl); for (typename vector<RT>::iterator itr = v_solutions.begin(); itr != v_solutions.end(); ++itr) { //collect votes int votes = 0; inliers.setZero(); P3 eivE; P3 pc; POINT_T cos_a; for (int c = 0; c < adapter.getNumberCorrespondences(); c++) { if (adapter.isValid(c)){ //with normal data POINT_T cos_alpha = adapter.getNormalCurr(c).dot(itr->so3().template cast<POINT_T>() * adapter.getNormalGlob(c)); if (cos_alpha > cos_nl_thre){ inliers(c, 2) = 1; votes++; } //with 3d data eivE = adapter.getPointCurr(c) - (itr->so3().template cast<POINT_T>() * adapter.getPointGlob(c) + itr->translation().template cast<POINT_T>()); if (eivE.norm() < thre_3d_){ inliers(c, 1) = 1; votes++; } } //with 2d pc = itr->so3().template cast<POINT_T>() * adapter.getPointGlob(c) + itr->translation().template cast<POINT_T>();// transform pw into pc pc = pc / pc.norm(); //normalize pc //compute the score cos_a = pc.dot( adapter.getBearingVector(c) ); if (cos_a > cos_thr){ inliers(c, 0) = 1; votes++; } } //cout << endl; if (votes > adapter.getMaxVotes() ){ assert(votes == inliers.sum()); adapter.setMaxVotes(votes); adapter.setRcw(itr->so3()); adapter.sett(itr->translation()); adapter.setInlier(inliers); //cout << inliers.inverse() << endl << endl; //adapter.printInlier(); //Iter = RANSACUpdateNumIters(confidence, (POINT_T)(adapter.getNumberCorrespondences() * 3 - votes) / adapter.getNumberCorrespondences() / 3, K, Iter); } }//for(vector<RT>::iterator itr = v_solutions.begin() ... }//for(int ii = 0; ii < Iter; ii++) PnPPoseAdapter<POSE_T, POINT_T>* pAdapterPnP = &adapter; pAdapterPnP->cvtInlier(); AOPoseAdapter<POSE_T, POINT_T>* pAdapterAO = &adapter; pAdapterAO->cvtInlier(); adapter.cvtInlier(); return; }