Camera Pose

[YJonmo]

Hi there,

Thanks for putting this work in public.

My question may sound silly, but do I need to have the camera pose to be able to use this repository? that's my impression by going through your codes.

What I have is just bunch of RGB-D images and I would like to fuse them to each other get the extended map.

Regards,
Jacob

[plutoyuxie]

hi, I have done some work like you mentioned.

1. set the first or certain frame as reference, its cam pose is np.eye(4)
2. transform all depth into camera space using camera intrinsics
3. use ICP to estimate the transform matrix for each frame to the reference in the world space, which is the cam pose of that frame

[YJonmo]

I have simulated data which I get them from Blender with corresponding matrix_world for the camera for every frame. But using this tool box I cannot reconstruct the same point cloud that I see in the Blender. It just shows a mess.

The Blender camera matrix_world is a 3*3 plus the 4rth column being the translation of x, y and z.

[ShrutheeshIR]

Hey @YJonmo , were you able to solve this issue? I revisited this code for a custom dataset, using simulated data from Gazebo and I, too am getting a messy point cloud. I do suspect there could be a problem with my transformation matrix, however, there is no way for me to verify if that is indeed the issue. (I obtained the transformation matrix from Gazebo itself)
Does it have something to do with the units of depth data, or something like that?
Please let me know if you have solved the issue

[YJonmo]

Yes mate, finally solved. There is a transformation conversion needed: VSLAMMappingFromBlender2DSO

Please check this repo: https://github.com/GSORF/Visual-GPS-SLAM/blob/master/02_Utilities/BlenderAddon/addon_vslam_groundtruth_Blender280.py#L34

[ShrutheeshIR]

Hi @YJonmo
Thanks! Since I am not using Blender, I couldn't understand the entire code snippet. A glance at the comments says that I need to convert it to this format.
Up = -Y-Axis
Right = X-Axis
Forward = Z-Axis.
The gazebo simulation format is such that Left is +X axis, and up is +Y axis. So I need to find an equivalent conversion right? Please correct me if I am wrong. For the same, if it is possible, could you let me know what to change in the code snippet that you have linked.
Thanks a ton!

[YJonmo]

I guess your format should be eventually like this:

    Up = -Y-Axis
    Right = X-Axis
    Forward = Z-Axis

above is the translation. You need to also convert the rotation matrix too (the 3x3). For converting the Blender below matrix was multiplied and then transposed:
1 0 0
0 -1 0
0 0 -1

In your case, you might need to convert the X to -X and Y to -Y.
In that case the rotation matrix should be multiplied to
-1 0 0
0 -1 0
0 0 1
But not transposed.
But to me, Gazebo is an environment for the computer vision and hence it should be compatible with SLAM or TSDF techniques by default, whereas Blender is not created for the computer vision and that's why conversion is needed.
I guess you should do some trial and error to find the correct conversion

[ShrutheeshIR]

Hello @YJonmo
That makes sense. I will try it out. Thanks once again!

[YJonmo]

These functions might be useful for you (conversion between the quaternion and the work matrix):
def quatr2world(Coordinates): #
X= np.float64(Coordinates[0])
Y= np.float64(Coordinates[1])
Z= np.float64(Coordinates[2])
W_O= np.float64(Coordinates[3])
X_O= np.float64(Coordinates[4])
Y_O= np.float64(Coordinates[5])
Z_O= np.float64(Coordinates[6])
World=np.zeros((4,4), np.float64)
World[0][0]= 1 - 2Y_OY_O - 2Z_OZ_O
World[0][1]= 2X_OY_O + 2W_OZ_O
World[0][2]= 2X_OZ_O - 2W_OY_O
World[1][0]= 2X_OY_O - 2W_OZ_O
World[1][1]= 1 - 2X_OX_O - 2Z_OZ_O
World[1][2]= 2Y_OZ_O + 2W_OX_O
World[2][0]= 2X_OZ_O + 2W_OY_O
World[2][1]= 2Y_OZ_O - 2W_OX_O
World[2][2]= 1 - 2X_OX_O - 2Y_OY_O
World[0][3]= X/1
World[1][3]= Y/1
World[2][3]= Z/1
World[3][3]= 1
World[0:3, 0:3] = np.transpose(World[0:3,0:3])
return World

def world2quatr(World): #
tr = World[0][0] + World[1][1] + World[2][2]
if (tr > 0):
S = np.sqrt(tr+1.0) * 2# // S=4qw
qw = 0.25 * S;
qx = (World[2][1] - World[1][2]) / S;
qy = (World[0][2] - World[2][0]) / S;
qz = (World[1][0] - World[0][1]) / S;
elif ((World[0][0] > World[1][1])and(World[0][0] > World[2][2])):
S = np.sqrt(1.0 + World[0][0] - World[1][1] - World[2][2]) * 2; # S=4qx
qw = (World[2][1] - World[1][2]) / S;
qx = 0.25 * S;
qy = (World[0][1] + World[1][0]) / S;
qz = (World[0][2] + World[2][0]) / S;
elif (World[1][1] > World[2][2]):
S = np.sqrt(1.0 + World[1][1] - World[0][0] - World[2][2]) * 2; # S=4qy
qw = (World[0][2] - World[2][0]) / S;
qx = (World[0][1] + World[1][0]) / S;
qy = 0.25 * S;
qz = (World[1][2] + World[2][1]) / S;
else:
S = np.sqrt(1.0 + World[2][2] - World[0][0] - World[1][1]) * 2; # S=4qz
qw = (World[1][0] - World[0][1]) / S;
qx = (World[0][2] + World[2][0]) / S;
qy = (World[1][2] + World[2][1]) / S;
qz = 0.25 * S;
quaternion=np.array([qw, qx, qy, qz])
return quaternion

[ShrutheeshIR]

What is Coordinates argument in the first function? Why does it have 6 values?

[YJonmo]

It has seven values. It is the quaternion coordinates [x y z qw qx qy qz ]

[ShrutheeshIR]

Oh makes sense.
That's awesome! Thanks

[Ademord]

@YJonmo i am trying to do something like this with unity, could you help me get my depth input into this repo's software?

[YJonmo]

@YJonmo i am trying to do something like this with unity, could you help me get my depth input into this repo's software?

What do you exactly want to do? like creating an extended 3D map using several depth frames?
Do you have the depth frames and their corresponding camera coordinates?

[GodZarathustra]

Yes mate, finally solved. There is a transformation conversion needed: VSLAMMappingFromBlender2DSO

Please check this repo: https://github.com/GSORF/Visual-GPS-SLAM/blob/master/02_Utilities/BlenderAddon/addon_vslam_groundtruth_Blender280.py#L34

Hi mate, I also have the same issues as you did. but the link you provided doesn't work for my side, actually, I am not sure what coordinate system is being used for this repo, could you provide the conversion matrix from blender coordinates to this repo's coordinates?
Thank you so much!