Quantcast

Plane Intersection

classic Classic list List threaded Threaded
11 messages Options
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Plane Intersection

inconnu259
Hi,

I use filter to detect some plan in my cloud point with SACMODEL_PLANE.
Now I search the intersection of 2 plane (so I search a line).
According to the doc : http://docs.pointclouds.org/trunk/group__sample__consensus.html
The four coefficients of the plane are its Hessian Normal form: [normal_x normal_y normal_z d].
But I'm not sure I understand what is the Hessian Normal form.
This is the normal vector of the plane and a coefficient?
Or other things ? And How can I have a simple equation of the plane like this ? :
ax+by+cz+d = 0 ???

I want to implement a method for finding the intersection between 2 planes, but perhaps exisiste their already something liks that in pcl ?

Cheers,

Thomas
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Plane Intersection

Radu B. Rusu
Administrator

On 03/12/2012 10:37 AM, inconnu259 wrote:

> Hi,
>
> I use filter to detect some plan in my cloud point with SACMODEL_PLANE.
> Now I search the intersection of 2 plane (so I search a line).
> According to the doc :
> http://docs.pointclouds.org/trunk/group__sample__consensus.html
> The four coefficients of the plane are its Hessian Normal form: [normal_x
> normal_y normal_z d].
> But I'm not sure I understand what is the Hessian Normal form.
> This is the normal vector of the plane and a coefficient?
> Or other things ? And How can I have a simple equation of the plane like
> this ? :
> ax+by+cz+d = 0 ???

Yes. You could have used any available search engine to find this (first result in most will lead to
http://mathworld.wolfram.com/HessianNormalForm.html) ;)

> I want to implement a method for finding the intersection between 2 planes,
> but perhaps exisiste their already something liks that in pcl ?

There doesn't seem to be one in the API, but it should be trivial to code. Let us know if you want to contribute it back
to PCL once you test it. Thanks in advance.

Cheers,
Radu.
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Plane Intersection

william.nguatem
Hi inconnu259,
how ugent do you the plane-plane intersection ?
I have to clean what I did some-time last year before committing for review in pcl community.
Actually the code I have is having a few dependencies which are to be replaced using Eigen.

Well, I can do this for you say in the comming couple of days if you would wait otherwise you can easily do the changes yourself and commit to pcl for review.

typedef pcl::PointCloud<pcl::PointXYZ>::Ptr  PtCloudDataPtr;

bool _PlanePlaneIntersection(const PtCloudDataPtr &PlaneA, const PtCloudDataPtr &PlaneB, Eigen::VectorXf &LineParams)
{
        Eigen::VectorXf fPlaneACoefs, fPlaneBCoefs;
        _LSPlaneFitting(PlaneA, fPlaneACoefs);    
        _LSPlaneFitting(PlaneB, fPlaneBCoefs);

        //plane normals
        Vec3d nA = {fPlaneACoefs[0], fPlaneACoefs[1], fPlaneACoefs[2]};
        Vec3d nB = {fPlaneBCoefs[0], fPlaneBCoefs[1], fPlaneBCoefs[2]};

        Math3d::NormalizeVec(nA);
        Math3d::NormalizeVec(nB);

        //planes should'nt be parallel
        if(VFRMath::IsParallel(nA, nB, 0.1))
                return false;

        Vec3d fLineDirection;
        Math3d::CrossProduct(nA, nB, fLineDirection);
        Math3d::NormalizeVec(fLineDirection);

        Eigen::Vector4f cA, cB;
        pcl::compute3DCentroid(*PlaneA, cA);
        pcl::compute3DCentroid(*PlaneB, cB);

        Vec3d fClosestPoint = {0.5*(cB[0] + cA[0]), 0.5*(cA[1] + cB[1]), 0.5*(cA[2] + cB[2])}; //mid-point of centroids

        Vec3d pA = {cA[0], cA[1], cA[2]};
        Vec3d pB = {cB[0], cB[1], cB[2]};

        float fValA = Math3d::ScalarProduct(pA,nA);
        float fValB = Math3d::ScalarProduct(pB,nB);

        Eigen::MatrixXf fLangegrangeCoefs(5,5);
        fLangegrangeCoefs << 2,0,0,nA.x,nB.x,  0,2,0,nA.y,nB.y,  0,0,2,nA.z,nB.z,  nA.x,nA.y,nA.z,0,0,  nB.x,nB.y,nB.z,0,0;  

        //std::cout << "Here is the matrix of the langrange equations minimized:\n" << fLangegrangeCoefs << std::endl;

        Eigen::VectorXf b;
        b.resize(5);
        b<< 2*fClosestPoint.x,2*fClosestPoint.y,2*fClosestPoint.z,fValA,fValB;
        //std::cout << "Here is the vector b:\n" << b << std::endl;
       
        Eigen::VectorXf x, xLineParams;
        xLineParams.resize(6);
        xLineParams[3] = fLineDirection.x;
        xLineParams[4] = fLineDirection.y;
        xLineParams[5] = fLineDirection.z;

        x.resize(5);
        x = fLangegrangeCoefs.colPivHouseholderQr().solve(b);

        LineParams.resize(6);
        LineParams[0] = x[0];
        LineParams[1] = x[1];
        LineParams[2] = x[2];
        LineParams[3] = fLineDirection.x;
        LineParams[4] = fLineDirection.y;
        LineParams[5] = fLineDirection.z;

        return true; //ToDo: do a better return
}





Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Plane Intersection

inconnu259
Thank you,

I look that today or tomorrow. If you want commit this, don't hesitate, if not I will. The problem is that I don't know where I can put it in pcl.
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Plane Intersection

inconnu259
In reply to this post by william.nguatem
Hi William,

I don't understand this :
LangegrangeCoefs << 2,0,0,nA.x,nB.x,  0,2,0,nA.y,nB.y,  0,0,2,nA.z,nB.z,  nA.x,nA.y,nA.z,0,0,  nB.x,nB.y,nB.z,0,0;  
Why do you have 5x5 matrix and not 3x3 ???
Just only nA.x, nB.x,          nA.y, nB.y,                nA.z, nB.z

and for b why this : 2*fClosestPoint.x,2*fClosestPoint.y,2*fClosestPoint.z,fValA,fValB;
and not only this : 2*fClosestPoint.x,2*fClosestPoint.y,2*fClosestPoint.z
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Plane Intersection

william.nguatem
Hi,
I happened to have time and went over the code. Below is a working copy. You shouldn't commit to PCL since it's not yet in accordance with one or two things on pcl coding policy e.g. spacing. Provided you have time to make this pcl conform, then you have to commit this into pcl/common and extend the already existing intersection.cpp/intersections.h files accordingly. I may not have time to do this prior to the weekend. And don't forget to give credits where it belongs by placing John krumm's paper as a reference. Sure, there're other ways to do plane-plane intersection, e.g using Plücker coordinates, but this is the simplest I came accross sofar.  

bool PlaneWithPlaneIntersection(const Eigen::Vector4f &fPlaneA, const Eigen::Vector4f &fPlaneB, Eigen::VectorXf &fLineCoefs)
{
        /**\brief Determine the line of intersection of two non-parallel planes using lagrange multipliers
        * \described in: "Intersection of Two Planes, John Krumm, Microsoft Research, Redmond, WA, USA"
        * \param[in] coefficients of plane A and Plane B in the form ax + by + cz + d = 0
        * \param[out] coefficients of line where fLinecoefs.tail<3>() = direction vector and
        * fLinecoefs.head<3>() the point on the line clossest to (0, 0, 0)
        * \return true if succeeded/planes aren't parallel
        */

        //planes shouldn't be parallel
        float fAngularTolerance = 0.1;//1e-5; //depending on how perfect you want things to be
        float fTestCosine = fPlaneA.head<3>().dot(fPlaneB.head<3>());
        float fUpperLimit = 1 + fAngularTolerance;
        float fLowerLimit = 1 - fAngularTolerance;

        if ((fTestCosine < fUpperLimit) && (fTestCosine > fLowerLimit))
        {
                PCL_ERROR ("Plane A and Plane B are Parallel");
                return (false);
        }

        if ((fTestCosine > -fUpperLimit) && (fTestCosine < -fLowerLimit))
        {
                PCL_ERROR ("Plane A and Plane B are Parallel");
                return (false);

        }

        Eigen::Vector4f &fLineDirection = fPlaneA.cross3(fPlaneB);
        fLineDirection.normalized();
       
        //reference point is (0,0,0)
        Eigen::Vector4f fRefToClosestPoint(0, 0, 0, 0);  

        //construct system of equations using lagrange multipliers with one objective function and two constraints
        Eigen::MatrixXf fLangegrangeCoefs(5,5);
        fLangegrangeCoefs << 2,0,0,fPlaneA[0],fPlaneB[0],  0,2,0,fPlaneA[1], fPlaneB[1],  0,0,2, fPlaneA[2], fPlaneB[2], fPlaneA[0], fPlaneA[1] , fPlaneA[2], 0,0, fPlaneB[0], fPlaneB[1], fPlaneB[2], 0,0;  

        Eigen::VectorXf b;
        b.resize(5);
        //b << 2*fRefToClosestPoint[0], 2*fRefToClosestPoint[1], 2*fRefToClosestPoint[2], PlaneA[3], PlaneB[3];
        b << 0, 0, 0, -fPlaneA[3], -fPlaneB[3];
       
        //solve for the lagrange Multipliers
        Eigen::VectorXf x;
        x.resize(5);
        x = fLangegrangeCoefs.colPivHouseholderQr().solve(b);

        fLineCoefs.resize(6);
        fLineCoefs.head<3>() = x.head<3>(); // the x[3] and x[4] are the values of the lagrange multipliers and are neglected
        fLineCoefs[3] = fLineDirection[0];
        fLineCoefs[4] = fLineDirection[1];
        fLineCoefs[5] = fLineDirection[2];
        return true;
}
 
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Plane Intersection

Radu B. Rusu
Administrator
If anyone has time to clean it up, and write an unit test to make sure it works fine, we'll be happy to push this in
trunk. Thanks in advance.

Cheers,
Radu.
--
http://pointclouds.org

On 03/13/2012 05:30 AM, william.nguatem wrote:

> Hi,
> I happened to have time and went over the code. Below is a working copy. You
> shouldn't commit to PCL since it's not yet in accordance with one or two
> things on pcl coding policy e.g. spacing. Provided you have time to make
> this pcl conform, then you have to commit this into pcl/common and extend
> the already existing intersection.cpp/intersections.h files accordingly. I
> may not have time to do this prior to the weekend. And don't forget to give
> credits where it belongs by placing John krumm's paper as a reference. Sure,
> there're other ways to do plane-plane intersection, e.g using Plücker
> coordinates, but this is the simplest I came accross sofar.  
>
> bool PlaneWithPlaneIntersection(const Eigen::Vector4f &fPlaneA, const
> Eigen::Vector4f &fPlaneB, Eigen::VectorXf &fLineCoefs)
> {
> /**\brief Determine the line of intersection of two non-parallel planes
> using lagrange multipliers
> * \described in: "Intersection of Two Planes, John Krumm, Microsoft
> Research, Redmond, WA, USA"
> * \param[in] coefficients of plane A and Plane B in the form ax + by + cz +
> d = 0
> * \param[out] coefficients of line where fLinecoefs.tail<3>() = direction
> vector and
> * fLinecoefs.head<3>() the point on the line clossest to (0, 0, 0)
> * \return true if succeeded/planes aren't parallel
> */
>
> //planes shouldn't be parallel
> float fAngularTolerance = 0.1;//1e-5; //depending on how perfect you want
> things to be
> float fTestCosine = fPlaneA.head<3>().dot(fPlaneB.head<3>());
> float fUpperLimit = 1 + fAngularTolerance;
> float fLowerLimit = 1 - fAngularTolerance;
>
> if ((fTestCosine < fUpperLimit) && (fTestCosine > fLowerLimit))
> {
> PCL_ERROR ("Plane A and Plane B are Parallel");
> return (false);
> }
>
> if ((fTestCosine > -fUpperLimit) && (fTestCosine < -fLowerLimit))
> {
> PCL_ERROR ("Plane A and Plane B are Parallel");
> return (false);
>
> }
>
> Eigen::Vector4f &fLineDirection = fPlaneA.cross3(fPlaneB);
> fLineDirection.normalized();
>
> //reference point is (0,0,0)
> Eigen::Vector4f fRefToClosestPoint(0, 0, 0, 0);  
>
> //construct system of equations using lagrange multipliers with one
> objective function and two constraints
> Eigen::MatrixXf fLangegrangeCoefs(5,5);
> fLangegrangeCoefs << 2,0,0,fPlaneA[0],fPlaneB[0],  0,2,0,fPlaneA[1],
> fPlaneB[1],  0,0,2, fPlaneA[2], fPlaneB[2], fPlaneA[0], fPlaneA[1] ,
> fPlaneA[2], 0,0, fPlaneB[0], fPlaneB[1], fPlaneB[2], 0,0;  
>
> Eigen::VectorXf b;
> b.resize(5);
> //b << 2*fRefToClosestPoint[0], 2*fRefToClosestPoint[1],
> 2*fRefToClosestPoint[2], PlaneA[3], PlaneB[3];
> b << 0, 0, 0, -fPlaneA[3], -fPlaneB[3];
>
> //solve for the lagrange Multipliers
> Eigen::VectorXf x;
> x.resize(5);
> x = fLangegrangeCoefs.colPivHouseholderQr().solve(b);
>
> fLineCoefs.resize(6);
> fLineCoefs.head<3>() = x.head<3>(); // the x[3] and x[4] are the values of
> the lagrange multipliers and are neglected
> fLineCoefs[3] = fLineDirection[0];
> fLineCoefs[4] = fLineDirection[1];
> fLineCoefs[5] = fLineDirection[2];
> return true;
> }
>  
>
> --
> View this message in context: http://www.pcl-users.org/Plane-Intersection-tp3819887p3822081.html
> Sent from the Point Cloud Library (PCL) Users mailing list archive at Nabble.com.
> _______________________________________________
> [hidden email] / http://pointclouds.org
> http://pointclouds.org/mailman/listinfo/pcl-users
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Plane Intersection

inconnu259
I think I can do it this week or this week end.
Just one question : where am I supposed to put the code?
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Plane Intersection

Radu B. Rusu
Administrator
Create an issue on http://dev.pointclouds.org and attach it there. Thank you in advance!

Cheers,
Radu.
--
http://pointclouds.org

On 03/14/2012 12:00 PM, inconnu259 wrote:
> I think I can do it this week or this week end.
> Just one question : where am I supposed to put the code?
>
> --
> View this message in context: http://www.pcl-users.org/Plane-Intersection-tp3819887p3826468.html
> Sent from the Point Cloud Library (PCL) Users mailing list archive at Nabble.com.
> _______________________________________________
> [hidden email] / http://pointclouds.org
> http://pointclouds.org/mailman/listinfo/pcl-users
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Plane Intersection

inconnu259
The new issue is here : http://dev.pointclouds.org/issues/644

Cheers,
Thomas
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Plane Intersection

link1609
This post has NOT been accepted by the mailing list yet.
Heyy,

the links dont work anymore.
Does anyone have the complete code?

Thank you,
Link
Loading...