convert line_detection output to pointcloud

[bsubei]

possible methods:

• [x] ~inverse perspective mapping~ Turns out this doesn't work for us because it doesn't use camera calibration and doesn't give us coordinates in real world
• [ ] the methods in our wiki page
• [ ] manually do it using some matrix solving in Matlab given the pinhole model here

[bsubei]

currently trying with projectPixelTo3dRay, which returns a 3dray corresponding to each pixel. Then, we can find the x and y values that intersect with a plane at a certain height.

But first, we have to deal with the fact that the coordinate systems of images are different. Check here.

• [x] create a static transform from camera_optical frame to camera that fixes the difference in frames. Just rotate +pi/2 for z and -pi/2 for y, done in this commit
• [ ] get the correct base_link to camera frame values (translation and rotation)

[bsubei]

• [x] create a node that:
  • [x] looks up transform from base_footprint -> base_link -> camera -> camera_optical
  • [x] finds rays for each pixel of image using projectPixelTo3dRay
  • [x] for debugging, create a pointcloud of all rays (just make them points with whatever arbitrary ray length) and view in rviz
  • [x] switch to tf2 instead of tf, perhaps that would fix all the ExtrapolationExceptions
  • [x] create a PointStamped object for each ray and transform it from camera_optical to base_footprint (inverse transform really).
  • [x] now that the points are in the base_footprint frame, find intersection of the ray (first point is origin of camera_optical frame relative to base_footprint, the second point is this new point we just got) with the ground plane (z = 0)
  • [x] find a way to get ROI params to control which pixels I find intersections for.
  • [x] fills up a 2d array of vec2f with (x,y) intersection coordinate for every pixel.
  • [x] write this 2d array to a file. Include the ROI information (and resolution, etc).
  • [x] TODO: these 3d ray values are for unrectified images, check rectified. From projectPixelTo3dRay: Returns the unit vector which passes from the camera center to through rectified pixel (u, v), using the camera P matrix. This means we have to first rectify the pixels before passing them into this function.
• [x] then write a node that:
  • [x] reads in the 2d array from file
  • [x] for every non-zero pixel in the input image (that we subscribe to), create a point in the pointcloud (with the x,y coordinates looked up from the 2d array and z=0)
  • [x] publish pointcloud

[bsubei]

essentially works. Needs precise testing.

[oxoocoffee]

Very nice…

On May 30, 2015, at 23:34, Basheer Subei notifications@github.com wrote:

essentially works. Needs precise testing.

— Reply to this email directly or view it on GitHub https://github.com/chicagoedt/Software_IGVC/issues/79#issuecomment-107127079.

[bsubei]

will test in Gazebo and real life