the pose of camera in rviz

[fboris]

After this commit, the pose of camera is totally opposite in rviz. It seems it publishes camera pose(down-looking) in the world frame. I found out this issue is related to

<param name="init_rx" value="3.14" />

Does that mean I need to rotate 180 degrees w.r.t x axis? If I want to see the camera in world frame. Moreover, does rpy2dcm generate the dcm by Euler angles(Roll, Pitch, Yaw)?

[cfo]

By convention the z-axis in the camera frame is orthogonal to the image plane (see https://github.com/uzh-rpg/rpg_svo/wiki/Notation). The parameters init_rx, inti_ry, init_rz allow to reposition the coordinate frame in the camera, e.g. if you want to align it with your coordinate system of your MAV.
The rpy convention we use is: Rz_Ry_Rx.

I suggest you make your own launchfile where you add <param name="init_rx" value="3.14" />

[fboris]

@cfo, thanks for your advice. I do a little experiment about these issue. I want my MAV can use svo for control. My first intuitive idea is to subscribe the /svo/pose/ node. But I still have question. Whatever I change the Euler angle(init_rx, init_ry, init_rz) will not affect to the ros topic /svo/pose. Nevertheless, it affect to the ros tpoic /tf which is visualized by rviz. Could I assume /svo/pose for svo algorithm and tf for rviz visualization?
If I leave publish_world_in_cam_frame to be the default value, /svo/pose will show a weird fame for me. I use rqt_plot to plot. It shows x point forward, y point right but z point up. In addition to, I set publish_world_in_cam_frame to False. It comes up a camera frame that you mentioned (https://github.com/uzh-rpg/rpg_svo/wiki/Notation). x point forward, y point right, x point down. Do I misunderstand something?

[fboris]

Actually, the rpy(init_rx, init_ry, init_rz) affect both /tf and /svo/pose.... Let me figure out first...

[enjoychang]

@cfo,
In visualizer.cpp, the way you compute pose orientation is

// publish cam in world frame
SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());
q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());

I see T_world_from_cam is computed by rotating the SVO pose by the user initial pose. T_world_from_vision_*frame->T_f_w_.inverse()
However, I don't quite understand when you computing q, why you multiply T_world_from_cam by T_world_from_vision_.rotation_matrix().transpose()?
It looks to me like that you are trying to rotate it back to SVO pose (then it should be the front multiplication)? I thought T_world_from_cam already contains the correct orientation for q.

[exialin]

@enjoychang I have the same question, and I think T_world_from_cam should be:
SE3 T_world_from_cam(T_world_from_vision_.inverse()*frame->T_f_w_.inverse());
Besides, I think T_world_from_vision_ should be replaced with T_world_from_vision_.inverse() in several functions.
Since T_world_from_vision_ is a diagonal matrix if set as <param name="init_rx" value="3.14" />, there are no differences. But it may not work properly if T_world_from_vision_ is not a diagonal matrix.