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 |
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 |
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 |
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 |
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? |
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 |
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); |
Free forum by Nabble - Resume Templates | Edit this page |