Fixed sac_model_plane to make sure a valid plane is optimized

Currently if you use `pcl::SACSegmentation` to find planes with some additional constraint attached (e.g. `SACMODEL_PERPENDICULAR_PLANE`), if `setOptimizeCoefficients (true)` is given there's a chance it will yield zero inliers even if there is very clearly a plane present. This is because `SACModelPlane::optimizeModelCoefficients` never tries to keep its result in the feasible set, and infeasible models yield zero inliers by design. Adding a tiny fix which says "If the optimized model is no longer in the feasible set, revert to the unoptimized one."