Eigen Points

Eigen

void simpleTest()
{
    Eigen::Vector3d ballBlue, ballRed;
    tf::pointMsgToEigen(pt1,ballBlue);
    tf::pointMsgToEigen(pt2,ballRed);
    // //Get distance between points 
    // Vector3 dist = ballBlue.Position - ballRed.Position; 
    Eigen::Vector3d dist = ballBlue - ballRed;    
    // dist.Normalize(); 
    dist.normalize();
 
    // //Get angle to arbitrary vector and compute the rotation axis 
    // float theta = (float)Math.Acos(Vector3.Dot(dist, Vector3.Up));
    Eigen::Vector3d up;
    up[0] = 0;
    up[1] = 1;
    up[2] = 0;
    double theta = (double)acos( dist.dot(up) );
    // Vector3 cross = Vector3.Cross(Vector3.Up, dist); 
    Eigen::Vector3d cross = up.cross(dist);
    // cross.Normalize(); 
    cross.normalize();
    // Quaternion rotation = Quaternion.CreateFromAxisAngle(cross, theta);
    Eigen::Affine3d output_vector;
    output_vector = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitY());
    output_vector.translation() = ballBlue;


      //Eigen::Affine3d output_vector;
      //output_vector = Eigen::Translation3d(O);
      //output_vector.rotate(m);

    ros::Duration(1.0).sleep();
    visual_tools_->publishArrow(output_vector, moveit_visualization_tools::RED, moveit_visualization_tools::REGULAR);
}