Restricting Transform Dimensions in Cloud Registration (ICP)

classic Classic list List threaded Threaded
7 messages Options
Reply | Threaded
Open this post in threaded view
|

Restricting Transform Dimensions in Cloud Registration (ICP)

barulicm
Hello,

I've been working with PCL this summer on a mobile robot equipped with a Kinect sensor.

I'm trying to stitch together frames from the Kinect into one map of the robot's environment. I'm using ICP to help fix errors in the odometry-based estimations. ICP usually gets the frames very close, but occasionally individual frames will be rotated in strange ways after ICP. (ie. one frame might be rotated as if the robot had fallen on it's side). This happens especially after a relatively complex set of motions (move forward - turn - move forward, etc.)

I know that, realistically, my robot can only translate along the ground plane and rotate about a single axis. Is there a way limit ICP's translation attempts to only these three dimensions in PCL?

Thank you,
--
~Matthew Barulic

_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Restricting Transform Dimensions in Cloud Registration (ICP)

Radu B. Rusu
Administrator
I think WarpPointRigid3D
(http://docs.pointclouds.org/trunk/classpcl_1_1_warp_point_rigid3_d.html#afc0569f74eb9c5255840be9a6a6e923b) might be
what you need.

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


On 07/17/2012 09:16 AM, Matt wrote:

> Hello,
>
> I've been working with PCL this summer on a mobile robot equipped with a Kinect sensor.
>
> I'm trying to stitch together frames from the Kinect into one map of the robot's environment. I'm using ICP to help fix
> errors in the odometry-based estimations. ICP usually gets the frames very close, but occasionally individual frames
> will be rotated in strange ways after ICP. (ie. one frame might be rotated as if the robot had fallen on it's side).
> This happens especially after a relatively complex set of motions (move forward - turn - move forward, etc.)
>
> I know that, realistically, my robot can only translate along the ground plane and rotate about a single axis. Is there
> a way limit ICP's translation attempts to only these three dimensions in PCL?
>
> Thank you,
> --
> ~Matthew Barulic
>
>
> _______________________________________________
> [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
|

Re: Restricting Transform Dimensions in Cloud Registration (ICP)

barulicm
Thanks for the suggestion.

I found your other mention of WarpPointRigid3D on the mailing list and it sounds like what I want. ("locking down a few degrees of freedom")
Are there any examples of using WarpPointRigid3D? I'm not a very advanced C++ programmer, and I'm not really sure just from the documentation how this can/should be used.

Thanks,
~Matt
Reply | Threaded
Open this post in threaded view
|

Re: Restricting Transform Dimensions in Cloud Registration (ICP)

barulicm
In reply to this post by Radu B. Rusu
Ok, I've spent some time looking at WarpPointRigid3D, but I'm still confused.

It looks to me like WarpPointRigid3D is a convenience class for making transform matrices that only effect the x, y, and rotation about z. How do I use this to restrict the types of motions that IterativeClosestPoint uses?

Thanks,
~Matt
Reply | Threaded
Open this post in threaded view
|

Re: Restricting Transform Dimensions in Cloud Registration (ICP)

blorgggg
Oooh, i would like to know about this too! I am using a modified version of what you normally do with ICP to test out Identification and position of ants that I am tracking (bio-tracking.org) from 2D video.

Before I would just use the X and Y coordinate of a src and target pointcloud and set all the Z's equal to zero. This kept everything in the XY plane.

Now I map the z position of a point to the hue. This distorts the color-coded ants and lets me ID them by comparing the fit of many models. The model with the lowest fitness score wins. Surprisingly this actually kinda works!

Of course, with this method, sometimes I run into problems where it will fit a model in a way that violates allowable rotations (only z rotation should be permitted) or it scoots the whole model up or down in the Z plane. Is there anyway I can tell ICP to try to align, but don't move in the disallowed directions?
Reply | Threaded
Open this post in threaded view
|

Re: Restricting Transform Dimensions in Cloud Registration (ICP)

TUMAAA
This post has NOT been accepted by the mailing list yet.
Hi,

I followed the discussion and there was no definitive answer. Did you succeed in limiting the ICP degrees of freedom? If so, how did you do it?

Thanks
Reply | Threaded
Open this post in threaded view
|

Re: Restricting Transform Dimensions in Cloud Registration (ICP)

Chris Flesher
Disclaimer: I just started using PCL so this may not be the most informed answer. I am working on a problem where I needed to adjust roll and pitch of two point clouds to align them. I figured out how to get this working by digging through the registration code.

First I created a custom warp class based off of WarpPointRigid, I called it WarpPointRollPitch and based it off of the code from WarpPoint3D. (I found that my two point clouds needed to be centered at zero for this to work)

#include <pcl/registration/warp_point_rigid.h>

namespace pcl
{
  namespace registration
  {
    /** \brief @b WarpPointRollPitch enables rotation in roll and pitch
      * transformations for points.
      */
    template <typename PointSourceT, typename PointTargetT, typename Scalar = float>
    class WarpPointRollPitch : public WarpPointRigid<PointSourceT, PointTargetT, Scalar>
    {
      public:
        typedef typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::Matrix4 Matrix4;
        typedef typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::VectorX VectorX;

        typedef boost::shared_ptr<WarpPointRollPitch<PointSourceT, PointTargetT, Scalar> > Ptr;
        typedef boost::shared_ptr<const WarpPointRollPitch<PointSourceT, PointTargetT, Scalar> > ConstPtr;

        /** \brief Constructor. */
        WarpPointRollPitch () : WarpPointRigid<PointSourceT, PointTargetT, Scalar> (2) {}
     
        /** \brief Empty destructor */
        virtual ~WarpPointRollPitch () {}

        /** \brief Set warp parameters.
          * \param[in] p warp parameters (tx ty rz)
          */
        virtual void
        setParam (const VectorX & p)
        {
          assert (p.rows () == this->getDimension ());
          Matrix4 &trans = this->transform_matrix_;
          trans (3, 3) = 1;
          Eigen::AngleAxis<Scalar> rollAngle(p[0], Eigen::Matrix<Scalar,3,1>(1,0,0));
          Eigen::AngleAxis<Scalar> pitchAngle(p[1], Eigen::Matrix<Scalar,3,1>(0,1,0));
          Eigen::Quaternion<Scalar> q = pitchAngle * rollAngle;
          trans.topLeftCorner (3, 3) = q.matrix();
        }
    };
  }
}

Then run ICP with the custom class:

    PointCloud::Ptr result (new PointCloud);
    Eigen::Matrix4f transform;
    pcl::IterativeClosestPoint<PointType, PointType> icp;
    typedef pcl::registration::TransformationEstimationLM <PointType, PointType>
        TransformationEstimationLM;
    typedef pcl::registration::WarpPointRollPitch<PointType, PointType> WarpFunction;
    WarpFunction::Ptr warpFunc (new WarpFunction);
    TransformationEstimationLM::Ptr transformEst (new TransformationEstimationLM);
    transformEst->setWarpFunction(warpFunc);
    icp.setTransformationEstimation(transformEst);
    icp.setInputSource(patch1);
    icp.setInputTarget(patch2);
    icp.align(*result);