Restricting Transform Dimensions in Cloud Registration (ICP)

Hello,
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users

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:
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users

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

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

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?

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

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);