Wrong goal Visualization

I think there is something wrong in the goal visulization code, regarding the rotations. The Wrong goal visualization can be seen in the first figure. This is happening after I merged #57 to my code
One arrow is by directly using addArrowMarker, and the other is using the goal Visulization
An IK solution is shown to verify the the addArrowMarker works.

The code to reproduce this is the following

`

auto base_to_object =
tf2::transformToEigen(lookup("base_link", "vicon_object_server/" + object));

auto epose = base_to_object * rpose;

// Region properties
auto tol = Eigen::Vector3d{0.001, 0.001, 0.001};
auto region = robowflex::Geometry::makeBox(0.01, 0.01, 0.01);
auto rot = Eigen::Quaterniond();

// Visualizing,
robowflex::MotionRequestBuilder vrequest(planner_, GROUP);

// // // wrist_roll_link is the default end effector
vrequest.setGoalRegion("wrist_roll_link", "base_link", epose, region, rot, tol);

rviz.removeMarker("goal");
rviz.removeMarker("test");

rviz.addGoalMarker("test", vrequest);
rviz.addArrowMarker("goal", "base_link", epose, Eigen::Vector4d{0.2, 0.3, 0.7, 1.0},
Eigen::Vector3d{0.1, 0.008, 0.003});
rviz.updateMarkers();

waitForUserConfirmation("Find the IK solution");

// Pulling the current scene
arm_with_torso_.pullScene(scene_);

// Finding the an IK solution
if (!fetch_->setFromIKCollisionAware(scene_, GROUP, region, epose, rot, tol))
{
ROS_WARN("No IK solution found for %s aborting ...", object.c_str());
return false;
}

rviz.visualizeCurrentState();
request.setGoalConfiguration(fetch_->getScratchState());

`