int PatchFit::fit() { // first check whether cloud subsampling is required RandomSample<PointXYZ> *rs; rs = new RandomSample<PointXYZ>(); if (this->ssmax_) { rs->setInputCloud(cloud_); rs->setSample(ssmax_); rs->setSeed(rand()); rs->filter(*cloud_); } delete (rs); //remove NAN points from the cloud std::vector<int> indices; removeNaNFromPointCloud(*cloud_, *cloud_, indices); //is_dense should be false // Step 1: fit plane by lls Vector4d centroid; compute3DCentroid(*cloud_, centroid); p_->setC(centroid.head(3)); cloud_vec.resize(cloud_->points.size(),3); fit_cloud_vec.resize(cloud_->points.size(),3); int vec_counter = 0; for (size_t i = 0; i<cloud_->points.size (); i++) { cloud_vec.row(vec_counter) = cloud_->points[i].getVector3fMap ().cast<double>(); // for fitting sub the centroid fit_cloud_vec.row(vec_counter) = cloud_vec.row(vec_counter) - centroid.head(3).transpose(); vec_counter++; } cloud_vec.conservativeResize(vec_counter-1, 3); //normal resize does not keep old values VectorXd b(fit_cloud_vec.rows()); b.fill(0.0); Vector3d zl = lls(fit_cloud_vec,b); p_->setR(zh.cross(zl)); double ss = p_->getR().norm(); double cc = zh.adjoint()*zl; double t = atan2(ss,cc); double th = sqrt(sqrt(EPS))*10.0; double aa; if (t>th) aa = t/ss; else aa = 6.0/(6.0-t*t); p_->setR(aa*p_->getR()); // Save the zl and the centroid p.c, for patch center constraint's use plane_n = zl; plane_c = p_->getC(); // Step 2: continue surface fitting if not plane VectorXd a; if (st_a) // general paraboloid { if (ccon_) { a.resize(6); a << 0.0, 0.0, p_->getR(), 0.0; } else { a.resize(8); a << 0.0, 0.0, p_->getR(), p_->getC(); } a = wlm(cloud_vec, a); // update the patch p_->setR(a.segment(2,3)); if (ccon_) p_->setC(plane_c + a(5)*plane_n); else p_->setC(a.segment(5,3)); Vector2d k; k << a(0), a(1); // refine into a specific paraboloid type if (k.cwiseAbs().maxCoeff() < this->ktol_) { // fitting planar paraboloid p_->setName("planar paraboloid"); p_->setS(Patch::s_p); Matrix<double,1,1> k_p; k_p << 0.0; p_->setK(k_p); p_->setR(p_->rcanon2(p_->getR(),2,3)); //TBD not updating correctly } else if (k.cwiseAbs().minCoeff() < this->ktol_) { // fitting cylindric paraboloid p_->setName("cylindric paraboloid"); p_->setS(Patch::s_y); p_->setB(Patch::b_r); kx = k(0); ky = k(1); if (fabs(kx) > fabs(ky)) { // swap curvatures kx = k(1); ky = k(0); Matrix3d ww; ww << yh, -xh, zh; Matrix3d rr; rr = p_->rexp(p_->getR()); rr = rr*ww; p_->setR(p_->rlog(rr)); } Eigen::Matrix<double,1,1> k_c_p; k_c_p << ky; p_->setK(k_c_p); } else if (fabs(k(0)-k(1)) < this->ktol_) { // fitting circular paraboloid p_->setName("circular paraboloid"); p_->setS(Patch::s_o); p_->setB(Patch::b_c); Matrix<double,1,1> k_c_p; k_c_p << k.mean(); p_->setK(k_c_p); p_->setR(p_->rcanon2(p_->getR(),2,3)); } else { if (sign(k(0),k(1))) { // fitting elliptic paraboloid p_->setName("elliptic paraboloid"); p_->setS(Patch::s_e); } else { // fitting hyperbolic paraboloid p_->setName("hyperbolic paraboloid"); p_->setS(Patch::s_h); } p_->setB(Patch::b_e); p_->setK(k); kx = k(0); ky = k(1); if (fabs(kx) > fabs(ky)) { // swap curvatures Vector2d k_swap; k_swap << ky, kx; p_->setK(k_swap); Matrix3d ww; ww << yh, -xh, zh; Matrix3d rr; rr = p_->rexp(p_->getR()); rr = rr*ww; p_->setR(p_->rlog(rr)); } } } else { // plane fitting p_->setS(Patch::s_p); p_->setB(this->bt_); Matrix<double,1,1> k_p; k_p << 0.0; p_->setK(k_p); } // Step 5: fit boundary; project to local frame XY plane Xform3 proj; Matrix3d rrinv; rrinv = p_->rexp(-p_->getR()); MatrixXd ux(cloud_vec.rows(),1), uy(cloud_vec.rows(),1), uz(cloud_vec.rows(),1); ux = cloud_vec.col(0); uy = cloud_vec.col(1); uz = cloud_vec.col(2); proj.transform(ux,uy,uz,p_->rexp(-p_->getR()), (-rrinv*p_->getC()),0,0); // (normalized) moments double mx = ux.mean(); double my = uy.mean(); double vx = (ux.array().square()).mean(); double vy = (uy.array().square()).mean(); double vxy = (ux.array() * uy.array()).mean(); // Step 5 lambda = sqrt(2.0) * boost::math::erf_inv(this->bcp_); //STEP 6: cylindric parab: aa rect bound. This step also sets p.c as the 1D // data centroid along the local frame x axis, which is the symmetry axis of // the cylinder. if (p_->getS() == Patch::s_y) { // fitting boundary: cyl parab, aa rect VectorXd d(2); d << lambda * sqrt(vx-mx*mx) , lambda * (sqrt(vy)); p_->setD(d); p_->setC(p_->rexp(p_->getR())*mx*xh + p_->getC()); } // STEP 7: circ parab: circ bound if (p_->getS() == Patch::s_o) { // fitting boundary: cir parab, circ VectorXd d(1); d << lambda * max(sqrt(vx),sqrt(vy)); p_->setD(d); } // STEP 8: ell or hyp parab: ell bound if (p_->getS() == Patch::s_e || p_->getS() == Patch::s_h) { // fitting boundary: ell/hyb parab, ell VectorXd d(2); d << lambda * sqrt(vx), lambda * sqrt(vy); p_->setD(d); } // STEP 9: plane bounds if (p_->getS() == Patch::s_p) { // fitting boundary: plane Matrix3d rr = p_->rexp(p_->getR()); p_->setC(rr*(mx*xh+my*yh) + p_->getC()); double a = vx-mx*mx; double b = 2.0*(vxy-mx*my); double c = vy-my*my; lambda = -log(1.0-this->bcp_); double d = sqrt(b*b+(a-c)*(a-c)); double wp = a+c+d; double wn = a+c-d; Vector2d l; l << sqrt(lambda*wp), sqrt(lambda*wn); VectorXd d_p(1); switch (p_->getB()) { case Patch::b_c: d_p << l.maxCoeff(); p_->setD(d_p); break; case Patch::b_e: p_->setD(l); break; case Patch::b_r: p_->setD(l); break; default: cerr << "Invalid pach boundary for plane" << endl; } if (p_->getB() != Patch::b_c) { double t = 0.5 * atan2(b,a-c); Matrix3d rr = p_->rexp(p_->getR()); Vector3d xl = rr.col(0); Vector3d yl = rr.col(1); Vector3d zl = rr.col(2); xl = (cos(t)*xl) + (sin(t)*yl); yl = zl.cross(xl); Matrix3d xyzl; xyzl << xl, yl, zl; p_->setR(p_->rlog(xyzl)); } } //plane boundary return (1); }