void doArcball(
double * _viewMatrix,
double const * _rotationCenter,
double const * _projectionMatrix,
double const * _initialViewMatrix,
//double const * _currentViewMatrix,
double const * _initialMouse,
double const * _currentMouse,
bool screenSpaceRadius,
double radius)
{
Eigen::Map<Eigen::Vector3d const> rotationCenter(_rotationCenter);
Eigen::Map<Eigen::Matrix4d const> initialViewMatrix(_initialViewMatrix);
//Eigen::Map<Eigen::Matrix4d const> currentViewMatrix(_currentViewMatrix);
Eigen::Map<Eigen::Matrix4d const> projectionMatrix(_projectionMatrix);
Eigen::Map<Eigen::Matrix4d> viewMatrix(_viewMatrix);
Eigen::Vector2d initialMouse(_initialMouse);
Eigen::Vector2d currentMouse(_currentMouse);
Eigen::Quaterniond q;
Eigen::Vector3d Pa, Pc;
if (screenSpaceRadius) {
double aspectRatio = projectionMatrix(1, 1) / projectionMatrix(0, 0);
initialMouse(0) *= aspectRatio;
currentMouse(0) *= aspectRatio;
Pa = mapToSphere(initialMouse, radius);
Pc = mapToSphere(currentMouse, radius);
q.setFromTwoVectors(Pa - rotationCenter, Pc - rotationCenter);
}
else {
Pa = mapToSphere(projectionMatrix.inverse(), initialMouse, rotationCenter, radius);
Pc = mapToSphere(projectionMatrix.inverse(), currentMouse, rotationCenter, radius);
Eigen::Vector3d a = Pa - rotationCenter, b = Pc - rotationCenter;
#if 0
std::cout << "Mouse Initial Radius = " << sqrt(a.dot(a)) << " Current Radius = " << sqrt(b.dot(b)) << std::endl;
#endif
q.setFromTwoVectors(a, b);
}
Eigen::Matrix4d qMat4;
qMat4.setIdentity();
qMat4.topLeftCorner<3, 3>() = q.toRotationMatrix();
Eigen::Matrix4d translate;
translate.setIdentity();
translate.topRightCorner<3, 1>() = -rotationCenter;
Eigen::Matrix4d invTranslate;
invTranslate.setIdentity();
invTranslate.topRightCorner<3, 1>() = rotationCenter;
viewMatrix = invTranslate * qMat4 * translate * initialViewMatrix;
}
Eigen::Quaterniond VizHelperNode::rotateSatelliteFrame(const iri_common_drivers_msgs::SatellitePseudorange &sat, ros::Time time_ros)
{
Eigen::Quaterniond rotation;
Eigen::Vector3d satVelocity(sat.v_x * scale, sat.v_y * scale, sat.v_z * scale);
//quaternione che fa ruotare l'asse x in modo che combaci con il vettore velocita'
rotation.setFromTwoVectors(Eigen::Vector3d::UnitX(), satVelocity);
geometry_msgs::Quaternion odom_quat;
odom_quat.x = rotation.x();
odom_quat.y = rotation.y();
odom_quat.z = rotation.z();
odom_quat.w = rotation.w();
// Publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = time_ros;
odom_trans.header.frame_id = WORLD_FRAME; //frame padre
odom_trans.child_frame_id = getSatelliteFrame(sat.sat_id); //frame figlio (che sto creando ora)
odom_trans.transform.translation.x = sat.x * scale;//traslazione dell'origine
odom_trans.transform.translation.y = sat.y * scale;
odom_trans.transform.translation.z = sat.z * scale;
odom_trans.transform.rotation = odom_quat; //rotazione
//send the transform
transBroadcaster.sendTransform(odom_trans);
return rotation;
}
bool Footcoords::updateGroundTF()
{
// project `/odom` on the plane of midcoords_
// odom -> midcoords is available as `midcoords_`
// we need odom -> odom_on_ground
// 1. in order to get translation,
// project odom point to
Eigen::Affine3d odom_to_midcoords;
tf::transformTFToEigen(midcoords_, odom_to_midcoords);
Eigen::Affine3d midcoords_to_odom = odom_to_midcoords.inverse();
Eigen::Affine3d midcoords_to_odom_on_ground = midcoords_to_odom;
midcoords_to_odom_on_ground.translation().z() = 0.0;
Eigen::Vector3d zaxis;
zaxis[0] = midcoords_to_odom_on_ground(0, 2);
zaxis[1] = midcoords_to_odom_on_ground(1, 2);
zaxis[2] = midcoords_to_odom_on_ground(2, 2);
Eigen::Quaterniond rot;
rot.setFromTwoVectors(zaxis, Eigen::Vector3d(0, 0, 1));
midcoords_to_odom_on_ground.rotate(rot);
Eigen::Affine3d odom_to_odom_on_ground
= odom_to_midcoords * midcoords_to_odom_on_ground;
tf::transformEigenToTF(odom_to_odom_on_ground,
ground_transform_);
}
void publishPlanes(ros::Publisher& plane_publisher,
keyframe_bundle_adjustment::BundleAdjusterKeyframes::Ptr bundle_adjuster,
std::string tf_parent_frame_id) {
visualization_msgs::MarkerArray out;
double plane_size = 6.;
// std::cout << "Plane positions:" << std::endl;
int count = 0;
for (const auto& id__kf_ptr : bundle_adjuster->keyframes_) {
if (id__kf_ptr.second->local_ground_plane_.distance > -0.1) {
visualization_msgs::Marker marker;
marker.header.frame_id = tf_parent_frame_id;
ros::Time cur_ts;
cur_ts.fromNSec(id__kf_ptr.second->timestamp_);
marker.header.stamp = cur_ts;
marker.type = visualization_msgs::Marker::CUBE;
marker.action = visualization_msgs::Marker::ADD;
marker.ns = std::to_string(count);
marker.scale.x = plane_size;
marker.scale.y = plane_size;
marker.scale.z = 0.1;
marker.color.a = 0.3; // Don't forget to set the alpha!
marker.color.r = 0.5;
marker.color.g = 0.5;
marker.color.b = 0.0;
Eigen::Map<Eigen::Vector3d> plane_dir(id__kf_ptr.second->local_ground_plane_.direction.data());
Eigen::Quaterniond ori;
ori.setFromTwoVectors(Eigen::Vector3d(0., 0., 1.), plane_dir);
marker.pose.orientation.x = ori.x();
marker.pose.orientation.y = ori.y();
marker.pose.orientation.z = ori.z();
marker.pose.orientation.w = ori.w();
Eigen::Vector3d pos = id__kf_ptr.second->getEigenPose().inverse().translation() -
plane_dir * id__kf_ptr.second->local_ground_plane_.distance;
marker.pose.position.x = pos.x();
marker.pose.position.y = pos.y();
marker.pose.position.z = pos.z();
// std::cout << pos.transpose() << "---" << plane_dir.transpose() << std::endl;
out.markers.push_back(marker);
count++;
{
visualization_msgs::Marker marker_arr;
marker_arr.header.frame_id = tf_parent_frame_id;
marker_arr.header.stamp = cur_ts;
marker_arr.type = visualization_msgs::Marker::ARROW;
marker_arr.action = visualization_msgs::Marker::ADD;
marker_arr.ns = std::to_string(count);
marker_arr.scale.x = 3.;
marker_arr.scale.y = 0.3;
marker_arr.scale.z = 0.3;
marker_arr.color.a = 0.7; // Don't forget to set the alpha!
marker_arr.color.r = 0.5;
marker_arr.color.g = 0.5;
marker_arr.color.b = 0.0;
Eigen::Quaterniond ori_arr;
ori_arr.setFromTwoVectors(Eigen::Vector3d(1., 0., 0.), plane_dir);
marker_arr.pose.orientation.x = ori_arr.x();
marker_arr.pose.orientation.y = ori_arr.y();
marker_arr.pose.orientation.z = ori_arr.z();
marker_arr.pose.orientation.w = ori_arr.w();
marker_arr.pose.position = marker.pose.position;
out.markers.push_back(marker_arr);
count++;
}
}
}
// std::cout << "Markers size=" << out.markers.size() << std::endl;
plane_publisher.publish(out);
}
