Request for help with understanding the poses

[SM1991CODES]

Hi,

First, thank you so much for this great effort. Wonderful work!!

Now, I am trying to do motion compensation across frame n, n-1, n-2 ...
However, being a beginner in robotics, I have some questions regarding the poses:

1. The poses are in camera coordinates as I see from the site, so they should be converted to velodyne before applying, is this correct? I think the generate_sequential.py file shows this?

2. What is the reference of each pose P[i] - robot/ velodyne pose wrt to world coordinates or wrt previous pose p[i-1]?
   Here again, I tried referring the generate_sequential.py script, but, I do not see a lot of difference after applying the compensation step, the concatenated point clouds look almost similar to what they look without any motion compensation (not applying the pose matrix).
   Ideally, I expect, after motion compensation, all static points should overlap to a large extent, leaving only mobile objects appearing multiple times. Is this expectation correct?

Please help me understand this. I would really appreciate if you could present a small example as well.

Best Regards
Sambit

[jbehley]

1. The poses.txt is given in the camera coordinate system, therefore, one has to convert it in the velodyne coordinate system, like:

semantic-kitti-api/generate_sequential.py

Lines 42 to 68 in c2d7712

	def parse_poses(filename, calibration):
	""" read poses file with per-scan poses from given filename
	Returns
	-------
	list
	list of poses as 4x4 numpy arrays.
	"""
	file = open(filename)
	poses = []
	Tr = calibration["Tr"]
	Tr_inv = inv(Tr)
	for line in file:
	values = [float(v) for v in line.strip().split()]
	pose = np.zeros((4, 4))
	pose[0, 0:4] = values[0:4]
	pose[1, 0:4] = values[4:8]
	pose[2, 0:4] = values[8:12]
	pose[3, 3] = 1.0
	poses.append(np.matmul(Tr_inv, np.matmul(pose, Tr)))
	return poses

Line 66 does the conversion:

      poses.append(np.matmul(Tr_inv, np.matmul(pose, Tr))) 

where Tr is the extrinsic calibration matrix from velodyne to camera.

2. poses are given in world reference frame. First pose is given at (0,0,0) and remaining poses are relative to this. As shown in the script, you can get the accumulated point cloud with respect to the current scan by "substracting" the current pose:

diff = np.matmul(inv(pose), past["pose"])

And yes, if you done the math correctly, then the static parts should be at the same place.

hope that helps.

[SM1991CODES]

Thank you so much for the guidance.
Here is what I get before and after motion compensating frames 2, 1, 0 (0 and 1 are projected to 2).
Is this what you would also expect? Or could it be better?

Before:

After:

We can see the 3 trees/ pillar like structures have come closer, but it is still not a perfect overlap. Also, the car at the right side near ego vehicle does not fully overlap.

Here is the code snippent:

`# load this frame and it's pose
frame_velo_pcl = np.fromfile(frame_path_velo, dtype=np.float32).reshape((-1, 4))
print("PCL size --> ", frame_velo_pcl.shape)
frame_velo_pose = poses[index] # the pose for this frame wTi
print("pose of frame {0} --> {1}".format(index, frame_velo_pose))

    # load index - 1 frame and it's pose
    past_index = index - 1
    frame_path_velo_past1 = path_frames_velodyne + '/' + frame_names_velo[past_index]
    frame_velo_pcl_past1 = np.fromfile(frame_path_velo_past1, dtype=np.float32).reshape((-1, 4))
    frame_velo_pose_past1 = poses[past_index]
    print("frame past1 velo shape --> ", frame_velo_pcl_past1.shape)
    print("pose frame past1 --> ", frame_velo_pose_past1)

    past_index2 = index - 2
    frame_path_velo_past2 = path_frames_velodyne + '/' + frame_names_velo[past_index2]
    frame_velo_pcl_past2 = np.fromfile(frame_path_velo_past2, dtype=np.float32).reshape((-1, 4))
    frame_velo_pose_past2 = poses[past_index2]

    # first just concat all 3 frames and plot
    pcl_cat = np.vstack([frame_velo_pcl, frame_velo_pcl_past1, frame_velo_pcl_past2])
    print("concated PCL shape --> ", pcl_cat.shape)
    mayavi.mlab.title("frames [n, n-1, n-2] -> no motion compensation")
    utils.draw_lidar(pcl_cat)
    mlab.show()

    # make it's BEV
    cat_frames_bev = birds_eye_point_cloud(pcl_cat, side_range=settings.y_range_bev, fwd_range=settings.x_range_bev, res=settings.cell_res_bev)

    # now motion compensate past frames
    frame_velo_pcl_past1_mcomp = np.dot(inv(frame_velo_pose), np.dot(frame_velo_pose_past1, frame_velo_pcl_past1.T)).T
    frame_velo_pcl_past2_mcomp = np.dot(inv(frame_velo_pose), np.dot(frame_velo_pose_past2, frame_velo_pcl_past2.T)).T
    pcl_cat_mcomp = np.vstack([frame_velo_pcl, frame_velo_pcl_past1_mcomp, frame_velo_pcl_past2_mcomp])
    utils.draw_lidar(pcl_cat_mcomp)
    mayavi.mlab.title("frames [n, n-1, n-2] -> motion compensated")
    mlab.show()`

Is this what you would also expect or is there something I have done wrong?
Please advise.
I have basically followed your code base in generate_sequential.py
frame_n --> world(pose_n) --> frame_p (pose_p_inv)

Thank you again for helping and looking forward to your comment.

Best Regards
Sambit

[jbehley]

No that's not expected.

With our poses, all frames should be properly aligned.

Let's say your poses are T1,T2, and T3. You now want to have everything in the reference frame of T3, then it's

T3' = inv(T3)*T3
T2' = inv(T3)*T2
T1' = inv(T3)*T1

Obviously T3' is the identity. Operationally you can think of this like this: You first move the points to world coordinates and then substract the pose of T3. If you do it correctly, then the prior frames should be properly aligned.

Hope that clarifies everything.

[SM1991CODES]

Thank you for the super fast reply.

I quickly gave it a try:

pose_past1_comp = np.dot(inv(frame_velo_pose), frame_velo_pose_past1) pose_past2_comp = p.dot(inv(frame_velo_pose), frame_velo_pose_past2) frame_velo_pcl_past1_mcomp = np.dot(pose_past1_comp, frame_velo_pcl_past1.T).T frame_velo_pcl_past2_mcomp = np.dot(pose_past2_comp, frame_velo_pcl_past2.T).T # frame_velo_pcl_past1_mcomp = np.dot(inv(frame_velo_pose), np.dot(frame_velo_pose_past1, frame_velo_pcl_past1.T)).T # frame_velo_pcl_past2_mcomp = np.dot(inv(frame_velo_pose), np.dot(frame_velo_pose_past2, frame_velo_pcl_past2.T)).T pcl_cat_mcomp = np.vstack([frame_velo_pcl, frame_velo_pcl_past1_mcomp, frame_velo_pcl_past2_mcomp]) utils.draw_lidar(pcl_cat_mcomp) mayavi.mlab.title("frames [n, n-1, n-2] -> motion compensated") mlab.show()

Here past_1 means frame - 1
past_2 means frame - 2

Unfortunately, I still see the same 'not perfect' alignment when I concatenate the point clouds (I use np.vstack()).
I am sorry to bother you again!!

frame_velo_pose --> T3 (frame3)
frame_velo_pose_past2 --> T2 (frame2)
frame_velo_pcl_past1 --> T1 (frame1)

So,
pose_past2_comp = np.dot(inv(frame_velo_pose), frame_velo_pose_past2) --> inv(T3)*T2
pose_past1_comp = np.dot(inv(frame_velo_pose), frame_velo_pose_past1) --> inv(T3)*T2

frame_velo_pcl_past1_mcomp = np.dot(pose_past1_comp, frame_velo_pcl_past1.T).T --> (inv(T3) * T2) * pcl_frame_1

Unfortunately I just don't see what am I doing different.
Could you please attach a few images of how it looks for you after compensating and concating 3 frames?

Best Regards
Sambit

[SM1991CODES]

I noted you mention about the subtraction operation. But as I understand, I do not really should be subtracting anything, right?
like no use of - operator as such, or?

[jbehley]

there is no substraction. it was meant as metaphor.

Please refer to: https://github.com/PRBonn/semantic-kitti-api/blob/master/generate_sequential.py

Specifically:

semantic-kitti-api/generate_sequential.py

Lines 180 to 202 in c2d7712

	for past in history:
	diff = np.matmul(inv(pose), past["pose"])
	tpoints = np.matmul(diff, past["points"].T).T
	tpoints[:, 3] = past["remissions"]
	tpoints = tpoints.reshape((-1))
	concated_points[4 * start:4 * (start + past["points"].shape[0])] = tpoints
	concated_labels[start:start + past["labels"].shape[0]] = past["labels"]
	start += past["points"].shape[0]
	# write scan and labels in one pass.
	concated_points.tofile(os.path.join(velodyne_folder, f))
	concated_labels.tofile(os.path.join(labels_folder, os.path.splitext(f)[0] + ".label"))
	# append current data to history queue.
	history.appendleft({
	"points": points,
	"labels": labels,
	"remissions": remissions,
	"pose": pose.copy()
	})

[SM1991CODES]

Okay, I shall give it a try again. Though I was actually referring this very code to begin with.
But, I thank you sincerely for this quick support.

I shall let you know as I make progress.

I have a strong suspicion about the way I concatenate the points after transforming. May be I am doing something wrong there.
I think the transform part is pretty clear from your explanation and the starter code.

Good Night and thank you, again!!

Best Regards
Sambit

[SM1991CODES]

Hi,

I am happy to inform - I found out the problem - I forgot to convert to homogeneous coordinates, hence the offsets.
How stupid of me!!!
Turns out I was doing the pose transforms fine, just that not in hom corrdinates.

Here is how it looks now

The static trees and poles are perfectly aligned now. The motorcycle can be seen moving.

Thank you a lot!!

Best Regards
Sambit

[jbehley]

Good that it works now. Good luck with everything that you now build ontop! :)

I will close now the issue.

[Flora279]

Hi,
May I ask how to fuse with future scans?
For example, current scan is n, I wish to fuse it not only with past scans n-1, n-2,... but also with n+1, n+2,....
How should I be approaching to this, is there any difference with fusing with past scans?

Thanks and looking forward to your reply.

[jbehley]

please have a look at the voxelizer (https://github.com/jbehley/voxelizer), where I create for scene completion a voxel grid of past and future scans. There are all necessary transformations included. You can have a look at the gen_data.cpp where this is achieved without the GUI, but it's as simple as having the pose of scan n and multiplying it with the inverse of (n+i, where i = -k, ..., -1, 1, ..., k), see https://github.com/jbehley/voxelizer/blob/0ace074b8981ded4c9bb73ad362b44ae2e57ad34/src/data/voxelize_utils.cpp#L183

hope that helps. If you have specific questions, then please open a new issue to also allow other to find it.