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;
}
