inline bool NormalPlane<PointT, NormalT>::doComputeModelCoefficients(const std::vector<int>& indices) { if (indices.size() != 3) { return false; } const PointT& p0 = Base::pointcloud_->at(indices[0]); const PointT& p1 = Base::pointcloud_->at(indices[1]); const PointT& p2 = Base::pointcloud_->at(indices[2]); const Eigen::Vector4f p1p0 = { p1.x - p0.x, p1.y - p0.y, p1.z - p0.z, 0.f }; const Eigen::Vector4f p2p0 = { p2.x - p0.x, p2.y - p0.y, p2.z - p0.z, 0.f }; /// Check for collinearity const Eigen::Vector4f dy1dy2 = p1p0.cwiseQuotient(p2p0); if ((dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1])) { return false; } Base::model_coefficients_.resize(4); Base::model_coefficients_[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1]; Base::model_coefficients_[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2]; Base::model_coefficients_[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0]; Base::model_coefficients_[3] = 0; Base::model_coefficients_.normalize(); float d = -1 * dot(p0); Base::model_coefficients_[3] = d; return true; }