orientation error using 6D pose estimate to display results in 3D

[huckl3b3rry87]

@j96w in #88, you mentioned that the canonical frame is the object mesh. If I load the points.xyz file into RVIZ, using PCL, the frame appears like this

for the mustard bottle. So, when the 6D estimate is all zeros, then the canonical frame is equal to the camera frame. But, when the cannonical frame needs to be shifted, I am seeing errors when I use the 6D pose estimate to show the result in 3D

where the RGB pose visualization estimate looks good

Then I publish the 6D pose of the object and use it to transform the points.xyz file from the camera frame to the 6D pose for visualization in RVIZ, then the pose estimate looks bad

As you can see, it is flipped and twisted, so the orientation is off

FYI: I have tried it several ways, including publishing the tf from the camera to the object then publishing the cloud in the object frame, and they have all given the same result.

First, I was thinking that I need to determine the initial pose of the object, by finding the centeriod of the PLY file and shifting the cloud using that result, similar to #88 yuxng/YCB_Video_toolbox#11 and yuxng/PoseCNN#103 , but I switched to directly using the points.xyz file,
without shifting and there is no difference.

Might this be due to differences in coordinate systems in RVIZ and the points.xyz file? For instance if I flip the z when I import the points.xyz file I get

In NVlabs/Deep_Object_Pose#101 @mintar mentions that the pose topic can be used for visualization, which looks like

But I would like to see the point clouds line up as this pose arrow does not help me evaluate accuracy of the orientation

@hoangcuongbk80 @j96w or @jontremblay or @yuxng or @MrLuer or @mintar any help with this question would be appreciated

[huckl3b3rry87]

Instead of using the .xyz file, I used the .obj meshfile, but the result is still poor.

where the script that I use to produce this is

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/conversions.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>   // TicToc
#include <pcl/filters/voxel_grid.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_eigen/tf2_eigen.h>
#include <pcl/point_types.h>
#include <tf_conversions/tf_eigen.h>
#include <Eigen/Geometry>

using namespace pcl;

typedef PointXYZRGBNormal PointT;
typedef PointCloud<PointT> PointCloudT;

ros::Subscriber pose_sub;
ros::Publisher pub_pc;

// parameters
std::string camera_frame_id;
std::string pose_topic, pose_pc_topic;

Eigen::Affine3d transform = Eigen::Affine3d::Identity();

void callback(const geometry_msgs::PoseStamped msg) {
    ROS_INFO_STREAM("Received pose: " << msg);

    tf::Pose pose;
    tf::poseMsgToTF(msg.pose, pose);
    const tf::Pose p2 = pose;
    tf::poseTFToEigen(p2, transform);

    return;
}

int main (int argc, char* argv[]){
  ros::init (argc, argv, "publish_pose_pc");   // initialize ROS
  ros::NodeHandle nh;

  nh.getParam("/frames/camera_frame_id", camera_frame_id);
  nh.getParam("/topics/pose_topic", pose_topic);
  nh.getParam("/topics/pose_pc_topic", pose_pc_topic);

  // read polygon textured mesh
  PointCloudT::Ptr cloud(new PointCloudT);
  TextureMesh::Ptr mesh(new TextureMesh);
  OBJReader obj;
  std::string mesh_filename = "../models/006_mustard_bottle/textured.obj";
  if (obj.read(mesh_filename, *mesh) == -1) {
    cout << "Could not read " << mesh_filename;
    return -1;
  }

  // convert polygon mesh to point cloud
  fromPCLPointCloud2(mesh->cloud, *cloud);

  // cloud shifted by pose estimate
  PointCloudT::Ptr transformed_cloud (new PointCloudT);

  pub_pc = nh.advertise<sensor_msgs::PointCloud2>(pose_pc_topic, 100);

  // subscribe to pose topic in camera_frame_id
  pose_sub = nh.subscribe(pose_topic, 1, callback);

  console::TicToc time;
  ros::Rate rate(10.0);
  while(ros::ok()){

    // transform the cloud
    transformPointCloud(*cloud, *transformed_cloud, transform);

    // convert to a ros message
    sensor_msgs::PointCloud2 pc_tr;
    toROSMsg(*transformed_cloud, pc_tr);

    // publish the point cloud for visualization in the camera_frame_id
    pc_tr.header.frame_id = camera_frame_id;
    pub_pc.publish(pc_tr);

    ros::spinOnce();
    rate.sleep();
  }
}

And pose is published from the NN as

# publish the pose of the object in the camera_frame
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = self.camera_frame_id
pose.pose.position = Point(my_t[0], my_t[1], my_t[2])  # 'my_t': translation
pose.pose.orientation = Quaternion(my_r[0], my_r[1], my_r[2], my_r[3]) #'my_r': quaternion
self.pub_pose.publish(pose)

[huckl3b3rry87]

I fixed the issue by changing the callback to

void callback(const geometry_msgs::PoseStamped msg) {
    ROS_INFO_STREAM("Received pose: " << msg);

    Eigen::Matrix4f T(Eigen::Matrix4f::Identity());
    Eigen::Vector3f trans;
    float rot_quaternion[4];
    trans(0) = msg.pose.position.x; trans(1) = msg.pose.position.y; trans(2) = msg.pose.position.z; //translaton
    rot_quaternion[0] = msg.pose.orientation.x; rot_quaternion[1] = msg.pose.orientation.y; //rotation
    rot_quaternion[2] = msg.pose.orientation.z; rot_quaternion[3] = msg.pose.orientation.w; //rotation

    Eigen::Quaternionf q(rot_quaternion[0], rot_quaternion[1], rot_quaternion[2], rot_quaternion[3]); //w x y z

    T.block(0, 3, 3, 1) = trans;
    T.block(0, 0, 3, 3) = q.normalized().toRotationMatrix();

    transform = T;
    return;
}

the code here helped. Thanks @hoangcuongbk80