Hello all;

I want to obtain parallel planes from an obtained plane useing the Sample Consensous module in PCL. I obtain the initial plane with the following code:

void ObtainMainPlane(void)
{
    //sceneCloudFiltered
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (planeDistanceThreshold); 
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    seg.setInputCloud (sceneCloudFiltered);
    seg.segment (*inliers, *coefficients);

    if (inliers->indices.size () == 0)
    {
        ROS_ERROR ("[%s] Could not estimate a planar model for the given dataset.",ros::this_node::getName().c_str());
    }
    else
    {
        std::cerr << "Model coefficients: " << coefficients->values[0] << " "<< coefficients->values[1] << " "
                    << coefficients->values[2] << " "<< coefficients->values[3] << std::endl; 
        std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;    

        plane_coeffs.clear();   
        plane_coeffs.push_back(coefficients->values[0]);
        plane_coeffs.push_back(coefficients->values[1]);
        plane_coeffs.push_back(coefficients->values[2]);
    if(coefficients->values[2] > 0)
      plane_coeffs.push_back(coefficients->values[3] + planeOffset);
    else if(coefficients->values[2] < 0)
      plane_coeffs.push_back(coefficients->values[3] - planeOffset);

    plane_obtained = true;
    }
}

As you can see in the code one I obtain the mail plane coefficients, I add an offset to the forth parameter obtained (the "d" parameter of the plane), taking into account the sign of the Uz parameter (the normal vector of the plane is nearly perpendicular to the obtained plane). However, results are not proving to be good. I do not see any difference between summing positive or negative values to this parameter.

Is my procedure correct? Should I use another way to obtain parallel planes from the original obtained? The axis of the camera are not parallel to the obtained plane, neither.

I would highly appreciate any kind of advise... Thank you all in advance !!