C++ Quaterniond::setFromTwoVectors方法代码示例

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);
}