PlaneExt::PlaneExt(but_plane_detector::Plane<float> plane) : but_plane_detector::Plane<float> (0.0, 0.0, 0.0, 0.0), planeCoefficients(new pcl::ModelCoefficients())
{
	a = plane.a;
	b = plane.b;
	c = plane.c;
	d = plane.d;
	norm = plane.norm;

	// Create a quaternion for rotation into XY plane
	Eigen::Vector3f current(a, b, c);
	Eigen::Vector3f target(0.0, 0.0, 1.0);
	Eigen::Quaternion<float> q;
	q.setFromTwoVectors(current, target);
	planeTransXY = q.toRotationMatrix();

	planeShift = -d;

	color.r = abs(a) / 2.0 + 0.2;
	color.g = abs(b) / 2.0 + 0.2;
	color.b = abs(d) / 5.0;
	color.a = 1.0;

	// Plane coefficients pre-calculation...
	planeCoefficients->values.push_back(a);
	planeCoefficients->values.push_back(b);
	planeCoefficients->values.push_back(c);
	planeCoefficients->values.push_back(d);

}

  bool testRawDataAcces() {
    bool passed = true;
    Eigen::Matrix<Scalar, 4, 1> raw = {0, 1, 0, 0};
    Eigen::Map<RxSO3Type const> map_of_const_rxso3(raw.data());
    SOPHUS_TEST_APPROX(passed, map_of_const_rxso3.quaternion().coeffs().eval(),
                       raw, Constants<Scalar>::epsilon());
    SOPHUS_TEST_EQUAL(passed, map_of_const_rxso3.quaternion().coeffs().data(),
                      raw.data());
    Eigen::Map<RxSO3Type const> const_shallow_copy = map_of_const_rxso3;
    SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(),
                      map_of_const_rxso3.quaternion().coeffs().eval());

    Eigen::Matrix<Scalar, 4, 1> raw2 = {1, 0, 0, 0};
    Eigen::Map<RxSO3Type> map_of_rxso3(raw.data());
    Eigen::Quaternion<Scalar> quat;
    quat.coeffs() = raw2;
    map_of_rxso3.setQuaternion(quat);
    SOPHUS_TEST_APPROX(passed, map_of_rxso3.quaternion().coeffs().eval(), raw2,
                       Constants<Scalar>::epsilon());
    SOPHUS_TEST_EQUAL(passed, map_of_rxso3.quaternion().coeffs().data(),
                      raw.data());
    SOPHUS_TEST_NEQ(passed, map_of_rxso3.quaternion().coeffs().data(),
                    quat.coeffs().data());
    Eigen::Map<RxSO3Type> shallow_copy = map_of_rxso3;
    SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(),
                      map_of_rxso3.quaternion().coeffs().eval());

    RxSO3Type const const_so3(quat);
    for (int i = 0; i < 4; ++i) {
      SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]);
    }

    RxSO3Type so3(quat);
    for (int i = 0; i < 4; ++i) {
      so3.data()[i] = raw[i];
    }

    for (int i = 0; i < 4; ++i) {
      SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]);
    }

    for (int i = 0; i < 10; ++i) {
      Matrix3<Scalar> M = Matrix3<Scalar>::Random();
      for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) {
        Matrix3<Scalar> R = makeRotationMatrix(M);
        Matrix3<Scalar> sR = scale * R;
        SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR),
                    "isScaledOrthogonalAndPositive(sR): % *\n%", scale, R);
        Matrix3<Scalar> sR_cols_swapped;
        sR_cols_swapped << sR.col(1), sR.col(0), sR.col(2);
        SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped),
                    "isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R);
      }
    }
    return passed;
  }

 // returns the local R,t in nd0 that produces nd1
 // NOTE: returns a postfix rotation; should return a prefix
 void transformN2N(Eigen::Matrix<double,4,1> &trans, 
                   Eigen::Quaternion<double> &qr,
                   Node &nd0, Node &nd1)
 {
   Matrix<double,3,4> tfm;
   Quaterniond q0,q1;
   q0 = nd0.qrot;
   transformW2F(tfm,nd0.trans,q0);
   trans.head(3) = tfm*nd1.trans;
   trans(3) = 1.0;
   q1 = nd1.qrot;
   qr = q0.inverse()*q1;
   qr.normalize();
   if (qr.w() < 0)
     qr.coeffs() = -qr.coeffs();
 }

Eigen::Quaternion<T> naive_slerp(const Eigen::Quaternion<T>& input, const Eigen::Quaternion<T>& target, T amt){
  Eigen::Quaternion<T> result;
	
  if (amt==T(0)) {
    return input;
  } else if (amt==T(1)) {
    return target;
  }
	
  int bflip = 0;
  T dot_prod = input.dot(target);
  T a, b;
	
  //clamp
  dot_prod = (dot_prod < -1) ? -1 : ((dot_prod > 1) ? 1 : dot_prod);
	
  // if B is on opposite hemisphere from A, use -B instead
  if (dot_prod < 0.0) {
    dot_prod = -dot_prod;
    bflip = 1;
  }
	
  T cos_angle = acos(dot_prod);
  if(ABS(cos_angle) > QUAT_EPSILON) {
    T sine = sin(cos_angle);
    T inv_sine = 1./sine;
		
    a = sin(cos_angle*(1.-amt)) * inv_sine;
    b = sin(cos_angle*amt) * inv_sine;
		
    if (bflip) { b = -b; }
  } else {
    // nearly the same;
    // approximate without trigonometry
    a = amt;
    b = 1.-amt;
  }
	
  result.w = a*input.w + b*target.w;
  result.x = a*input.x + b*target.x;
  result.y = a*input.y + b*target.y;
  result.z = a*input.z + b*target.z;
	
  result.normalize();
  return result;
}

    inline static void setFromVectors(Rotation_& rot, const Eigen::Matrix<PrimType_, 3, 1>& v1, const Eigen::Matrix<PrimType_, 3, 1>& v2) {
        KINDR_ASSERT_TRUE(std::runtime_error,  v1.norm()*v2.norm() != static_cast<PrimType_>(0.0), "At least one vector has zero length.");

        Eigen::Quaternion<PrimType_> eigenQuat;
        eigenQuat.setFromTwoVectors(v1, v2);
        rot = kindr::rotations::eigen_impl::RotationQuaternion<PrimType_, Usage_>(eigenQuat);
//
//    const PrimType_ angle = acos(v1.dot(v2)/temp);
//    const PrimType_ tol = 1e-3;
//
//    if(0 <= angle && angle < tol) {
//      rot.setIdentity();
//    } else if(M_PI - tol < angle && angle < M_PI + tol) {
//      rot = eigen_impl::AngleAxis<PrimType_, Usage_>(angle, 1, 0, 0);
//    } else {
//      const Eigen::Matrix<PrimType_, 3, 1> axis = (v1.cross(v2)).normalized();
//      rot = eigen_impl::AngleAxis<PrimType_, Usage_>(angle, axis);
//    }
    }

visualization_msgs::Marker SatDetector3DMonitor::getMarker(int marker_id, Eigen::Vector4f centroid, Eigen::Matrix3f covariance_matrix){

  //------- Compute Principal Componets for Marker publishing


  //Get the principal components and the quaternion
  Eigen::Matrix3f evecs;
  Eigen::Vector3f evals;
  //pcl::eigen33 (covariance_matrix, evecs, evals);
  Eigen::EigenSolver<Eigen::Matrix3f> es(covariance_matrix);
	
  evecs = es.eigenvectors().real();
  evals = es.eigenvalues().real();
	    
  Eigen::Matrix3f rotation;
  rotation.row (0) = evecs.col (0);
  rotation.row (1) = evecs.col (1);
  rotation.row (2) = rotation.row (0).cross (rotation.row (1));
	    
  rotation.transposeInPlace ();
  Eigen::Quaternion<float> qt (rotation);
  qt.normalize ();
	    
  //Publish Marker for cluster
  visualization_msgs::Marker marker;	
		
  marker.header.frame_id = base_frame_;
  marker.header.stamp = ros::Time().now();
  marker.ns = "Perception";
  marker.action = visualization_msgs::Marker::ADD;
  marker.id = marker_id;	
  marker.lifetime = ros::Duration(1);	
		
  //centroid position
  marker.type = visualization_msgs::Marker::SPHERE;
  marker.pose.position.x = centroid[0];
  marker.pose.position.y = centroid[1];
  marker.pose.position.z = centroid[2];	
  marker.pose.orientation.x = qt.x();
  marker.pose.orientation.y = qt.y();
  marker.pose.orientation.z = qt.z();
  marker.pose.orientation.w = qt.w();			

  //std::cout << "e1: " << evals(0) << " e2: " << evals(1) << " e3: " << evals(2) << std::endl;

  marker.scale.x = sqrt(evals(0)) * 4;
  marker.scale.y = sqrt(evals(1)) * 4;
  marker.scale.z = sqrt(evals(2)) * 4;
	

  //give it some color!
  marker.color.a = 0.75;
  satToRGB(marker.color.r, marker.color.g, marker.color.b);

  //std::cout << "marker being published" << std::endl;

  return marker;
}

IGL_INLINE void igl::two_axis_valuator_fixed_up(
  const int w,
  const int h,
  const double speed,
  const Eigen::Quaternion<Scalardown_quat> & down_quat,
  const int down_x,
  const int down_y,
  const int mouse_x,
  const int mouse_y,
  Eigen::Quaternion<Scalarquat> & quat)
{
  using namespace Eigen;
  Matrix<Scalarquat,3,1> axis(0,1,0);
  quat = down_quat *
    Quaternion<Scalarquat>(
      AngleAxis<Scalarquat>(
        PI*((Scalarquat)(mouse_x-down_x))/(Scalarquat)w*speed/2.0,
        axis.normalized()));
  quat.normalize();
  {
    Matrix<Scalarquat,3,1> axis(1,0,0);
    if(axis.norm() != 0)
    {
      quat =
        Quaternion<Scalarquat>(
          AngleAxis<Scalarquat>(
            PI*(mouse_y-down_y)/(Scalarquat)h*speed/2.0,
            axis.normalized())) * quat;
      quat.normalize();
    }
  }
}

    SE3<> IMU2camWorldfromQuat(Eigen::Quaternion<double> atti){// use ini attitude info from EKF node to ini ptam pose
        Vector<4> attivec = makeVector(atti.w(), atti.x(), atti.y(), atti.z());//Rw1i
        Matrix<3> iniOrientationEKF;
        iniOrientationEKF = tag::quaternionToMatrix(attivec);

        Matrix<3> roll = TooN::Data(1.0, 0, 0,//Rww1, because the roll and pitch angles are in
                              0, -1, 0, // a world frame which pointing downward.
                              0, 0, -1);

        SE3<> camWorld = SE3<>();
        Matrix<3> rotation;
        if (tracker_->attitude_get)
            rotation = iniOrientationEKF; //
        else
            rotation = roll * iniOrientationEKF;//Rwi = Rww1*Rw1i
        camWorld.get_rotation() = rotation*se3IMUfromcam.get_rotation().get_matrix();//Rwc = (Rwi * Ric)

        Vector<3> twr = makeVector(0.0, 0.0, 0.198);// twc = twr + Rwr * trc
        Vector<3> twc = twr + rotation * se3IMUfromcam.get_translation();
        camWorld.get_translation()[0] = 0.0;//twc[0]; //twc
        camWorld.get_translation()[1] = 0.0;//twc[1];
        camWorld.get_translation()[2] = twc[2];

        camWorld = camWorld.inverse();//Tcw

        cout<< "TCW INITIALIZED. TWC: " << twc[0]<< ", " << twc[1]<< ", " << twc[2]<<endl;
//        cout<< camWorld.get_rotation().get_matrix()(2,2)<<endl;
        return camWorld;
    }

void HLManager::onVTTwist(const geometry_msgs::TwistStamped::ConstPtr& twist)
{
	if (mode == circle || mode == vtManual)
	{
		//\todo Generalize this 0.1 with Ts.
		s +=twist->twist.linear.x*0.1;

		//Circle
		if (s>=2*circleRadius*M_PI) s=s-2*circleRadius*M_PI;
		else if (s<0) s=2*circleRadius*M_PI-s;

		double xRabbit = point.point.x + circleRadius*cos(s/circleRadius);
		double yRabbit = point.point.y + circleRadius*sin(s/circleRadius);
		double gammaRabbit=labust::math::wrapRad(s/circleRadius)+M_PI/2;

		tf::Transform transform;
		transform.setOrigin(tf::Vector3(xRabbit, yRabbit, 0));
		Eigen::Quaternion<float> q;
		labust::tools::quaternionFromEulerZYX(0,0,gammaRabbit, q);
		transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
		broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local", "serret_frenet_frame"));

		auv_msgs::NavStsPtr msg(new auv_msgs::NavSts());
		msg->position.north = xRabbit;
		msg->position.east = yRabbit;
		msg->body_velocity.x = twist->twist.linear.x;
		msg->orientation.yaw = gammaRabbit;
		sfPub.publish(msg);
	}
}

void HLManager::start()
{
	ros::Rate rate(10);

	while (nh.ok())
	{
		if (fixValidated)
		{
			tf::Transform transform;
			transform.setOrigin(tf::Vector3(originLon, originLat, 0));
			transform.setRotation(tf::createQuaternionFromRPY(0,0,0));
			broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/worldLatLon", "/world"));
			transform.setOrigin(tf::Vector3(0, 0, 0));
			Eigen::Quaternion<float> q;
			labust::tools::quaternionFromEulerZYX(M_PI,0,M_PI/2,q);
			transform.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
			broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "local"));
		}
		this->safetyTest();
		this->step();
		hlMessagePub.publish(hlDiagnostics);
		rate.sleep();
		ros::spinOnce();
	}
}

bool CloudMatcher::match(modular_cloud_matcher::MatchClouds::Request& req, modular_cloud_matcher::MatchClouds::Response& res)
{
	// get and check reference
	size_t referenceGoodCount;
	DP referenceCloud(rosMsgToPointMatcherCloud(req.reference, referenceGoodCount));
	const unsigned referencePointCount(req.reference.width * req.reference.height);
	const double referenceGoodRatio(double(referenceGoodCount) / double(referencePointCount));
	
	if (referenceGoodCount == 0)
	{
		ROS_ERROR("I found no good points in the reference cloud");
		return false;
	}
	if (referenceGoodRatio < 0.5)
	{
		ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - referenceGoodRatio*100.0 << "% of the cloud (received " << referenceGoodCount << ")");
	}
	
	// get and check reading
	size_t readingGoodCount;
	DP readingCloud(rosMsgToPointMatcherCloud(req.readings, readingGoodCount));
	const unsigned readingPointCount(req.readings.width * req.readings.height);
	const double readingGoodRatio(double(readingGoodCount) / double(readingPointCount));
	
	if (readingGoodCount == 0)
	{
		ROS_ERROR("I found no good points in the reading cloud");
		return false;
	}
	if (readingGoodRatio < 0.5)
	{
		ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - readingGoodRatio*100.0 << "% of the cloud (received " << readingGoodCount << ")");
	}
	
	// call icp
	TP transform;
	try 
	{
		transform = icp(readingCloud, referenceCloud);
		ROS_INFO_STREAM("match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl);
	}
	catch (PM::ConvergenceError error)
	{
		ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
		return false;
	}
	
	// fill return value
	res.transform.translation.x = transform.coeff(0,3);
	res.transform.translation.y = transform.coeff(1,3);
	res.transform.translation.z = transform.coeff(2,3);
	const Eigen::Quaternion<Scalar> quat(Matrix3(transform.block(0,0,3,3)));
	res.transform.rotation.x = quat.x();
	res.transform.rotation.y = quat.y();
	res.transform.rotation.z = quat.z();
	res.transform.rotation.w = quat.w();
	
	return true;
}

void quaternion2vector(const Eigen::Quaternion<T_qt> &quat_In, std::vector<T_vec> &quat_Vec )
{
    quat_Vec.resize(4,T_vec(0.0));
    quat_Vec[0] = T_vec( quat_In.w() );
    quat_Vec[1] = T_vec( quat_In.x() );
    quat_Vec[2] = T_vec( quat_In.y() );
    quat_Vec[3] = T_vec( quat_In.z() );
};

IGL_INLINE bool igl::snap_to_canonical_view_quat(
  const Eigen::Quaternion<Scalarq> & q,
  const double threshold,
  Eigen::Quaternion<Scalars> & s)
{
  return snap_to_canonical_view_quat<Scalars>(
    q.coeffs().data(),threshold,s.coeffs().data());
}

void SetTwistHandler::handleSimulation(){
    // called when the main script calls: simHandleModule
    if(!_initialized){
        _initialize();
    }

    Eigen::Quaternion < simFloat > orientation; //(x,y,z,w)
    Eigen::Matrix< simFloat, 3, 1> linVelocity((simFloat)_twistCommands.twist.linear.x,
    		(simFloat)_twistCommands.twist.linear.y,
    		(simFloat)_twistCommands.twist.linear.z);
    Eigen::Matrix< simFloat, 3, 1> angVelocity((simFloat)_twistCommands.twist.angular.x,
    		(simFloat)_twistCommands.twist.angular.y,
    		(simFloat)_twistCommands.twist.angular.z);

    // Input velocity is expected to be in body frame but V-Rep expects it to be in world frame.
    if(simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){
        linVelocity = orientation*linVelocity;
	    angVelocity = orientation*angVelocity;
    } else {
        std::stringstream ss;
        ss << "- [" << _associatedObjectName << "] Error getting orientation. " << std::endl;
        ConsoleHandler::printInConsole(ss);
    }

    simResetDynamicObject(_associatedObjectID);

	//Set object velocity
    if (_isStatic){
    	Eigen::Matrix<simFloat, 3, 1> position;
    	simGetObjectPosition(_associatedObjectID, -1, position.data());
    	const simFloat timeStep = simGetSimulationTimeStep();
    	position = position + timeStep*linVelocity;
    	simSetObjectPosition(_associatedObjectID, -1, position.data());
    	const simFloat angle = timeStep*angVelocity.norm();
    	if(angle > 1e-6){
    		orientation = Eigen::Quaternion< simFloat >(Eigen::AngleAxis< simFloat >(timeStep*angVelocity.norm(), angVelocity/angVelocity.norm()))*orientation;
    	}
    	simSetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data());
    } else {
		// Apply the linear velocity to the object
		if(simSetObjectFloatParameter(_associatedObjectID, 3000, linVelocity[0])
			&& simSetObjectFloatParameter(_associatedObjectID, 3001, linVelocity[1])
			&& simSetObjectFloatParameter(_associatedObjectID, 3002, linVelocity[2])==-1) {
			std::stringstream ss;
			ss << "- [" << _associatedObjectName << "] Error setting linear velocity. " << std::endl;;
			ConsoleHandler::printInConsole(ss);
		}
		// Apply the angular velocity to the object
		if(simSetObjectFloatParameter(_associatedObjectID, 3020, angVelocity[0])
			&& simSetObjectFloatParameter(_associatedObjectID, 3021, angVelocity[1])
			&& simSetObjectFloatParameter(_associatedObjectID, 3022, angVelocity[2])==-1) {
			std::stringstream ss;
			ss << "- [" << _associatedObjectName << "] Error setting angular velocity. " << std::endl;;
			ConsoleHandler::printInConsole(ss);
		}
    }

}

void GetTwistHandler::handleSimulation(){
    // called when the main script calls: simHandleModule
    if(!_initialized){
        _initialize();
    }

    ros::Time now = ros::Time::now();


    const simFloat currentSimulationTime = simGetSimulationTime();

    if ((currentSimulationTime-_lastPublishedObjTwistTime) >= 1.0/_ObjTwistFrequency){

    	Eigen::Quaternion< simFloat > orientation; //(x,y,z,w)
    	Eigen::Matrix<simFloat, 3, 1> linVelocity;
    	Eigen::Matrix<simFloat, 3, 1> angVelocity;
    	bool error = false;

    	// Get object velocity. If the object is not static simGetVelocity is more accurate.
    	if (_isStatic){
    		error = error || simGetObjectVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
    	} else {
    		error = error || simGetVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
    	}

    	// Get object orientation
    	error = error || simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data()) == -1;

    	if(!error){

    		linVelocity = orientation.conjugate()*linVelocity; // Express linear velocity in body frame
			angVelocity = orientation.conjugate()*angVelocity; // Express angular velocity in body frame

    		// Fill the status msg
			geometry_msgs::TwistStamped msg;

			msg.twist.linear.x = linVelocity[0];
			msg.twist.linear.y = linVelocity[1];
			msg.twist.linear.z = linVelocity[2];
			msg.twist.angular.x = angVelocity[0];
			msg.twist.angular.y = angVelocity[1];
			msg.twist.angular.z = angVelocity[2];

			msg.header.stamp = now;
			_pub.publish(msg);
			_lastPublishedObjTwistTime = currentSimulationTime;

    	} else {
    	    std::stringstream ss;
    	    ss << "- [" << _associatedObjectName << "] Error getting object velocity and/or orientation." << std::endl;;
    	    ConsoleHandler::printInConsole(ss);
    	}

    }

}

void YouBot::updateWheels(const ros::TimerEvent& e)
{
  try
  {
    if(q_base_.rows() != robot_->getMacroManipulatorDOF()) return;
    if(tau_base_.rows() != robot_->getMacroManipulatorDOF()) return;
    if(!gazebo_interface_wheel_->subscribed()) return;

    Eigen::VectorXd q = gazebo_interface_wheel_->getJointStates();
    robot_->updateWheel(q);

    Eigen::Vector3d base_pos;
    base_pos << q_base_[0], q_base_[1], 0.0;
    Eigen::Quaternion<double> base_ori;
    base_ori.x() = 0.0;
    base_ori.y() = 0.0;
    base_ori.z() = sin(0.5 * q_base_[2]);
    base_ori.w() = cos(0.5 * q_base_[2]);

    robot_->updateBase(base_pos, base_ori);

    Eigen::VectorXd v_base;
    controller_->computeBaseVelocityFromTorque(tau_base_, v_base, 3);

    Eigen::VectorXd v_wheel;
    controller_->computeWheelVelocityFromBaseVelocity(v_base, v_wheel);

    Eigen::VectorXd q_wheel_d = robot_->getMobility()->update_rate * v_wheel;

    std::vector<Eigen::Quaternion<double> > quat_d;
    for(unsigned int i = 0; i < q_wheel_d.rows(); ++i)
    {
      double rad = q_wheel_d[i];

      Eigen::Quaternion<double> quat;
      quat.x() = 0.0;
      quat.y() = sin(0.5 * rad);
      quat.z() = 0.0;
      quat.w() = cos(0.5 * rad);

      quat_d.push_back(quat);
    }

    gazebo_interface_wheel_->rotateLink(quat_d);
  }
  catch(ahl_robot::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
  catch(ahl_ctrl::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
}

 void transformF2W(Eigen::Matrix<double,3,4> &m, 
                   const Eigen::Matrix<double,4,1> &trans, 
                   const Eigen::Quaternion<double> &qrot)
 {
   m.block<3,3>(0,0) = qrot.toRotationMatrix();
   m.col(3) = trans.head(3);
 };

Eigen::Matrix< Tp, 4, 4 > transformationMatrix( Eigen::Quaternion< Tp > &quaternion, Eigen::Matrix< Tp, 3, 1 > &translation)
{
    Eigen::Matrix< Tp, 3, 3 > rr = quaternion.toRotationMatrix();
    Eigen::Matrix< Tp, 4, 4 > ttmm;
    ttmm << rr, translation, 0.0, 0.0, 0.0, 1.0;
    return ttmm;
}

    /**
     * @brief Computes the trackball's rotation, using stored initial and final position vectors.
     */
    void computeRotationAngle (void)
    {
        //Given two position vectors, corresponding to the initial and final mouse coordinates, calculate the rotation of the sphere that will keep the mouse always in the initial position.

        if(initialPosition.norm() > 0) {
            initialPosition.normalize();
        }
        if(finalPosition.norm() > 0) {
            finalPosition.normalize();
        }

        //cout << "Initial Position: " << initialPosition.transpose() << " Final Position: " << finalPosition.transpose() << endl << endl;

        Eigen::Vector3f rotationAxis = initialPosition.cross(finalPosition);

        if(rotationAxis.norm() != 0) {
            rotationAxis.normalize();
        }

        float dot = initialPosition.dot(finalPosition);

        float rotationAngle = (dot <= 1) ? acos(dot) : 0;//If, by losing floating point precision, the dot product between the initial and final positions is bigger than one, ignore the rotation.

        Eigen::Quaternion<float> q (Eigen::AngleAxis<float>(rotationAngle,rotationAxis));

        quaternion = q * quaternion;
        quaternion.normalize();
    }

    void rpyToQuaternion(const Eigen::Vector3d& rpy, Eigen::Quaternion<double>& q)
    {
      double a = rpy.coeff(0);
      double b = rpy.coeff(1);
      double g = rpy.coeff(2);

      double sin_b_half = sin(0.5 * b);
      double cos_b_half = cos(0.5 * b);
      double diff_a_g_half = 0.5 * (a - g);
      double sum_a_g_half = 0.5 * (a + g);

      q.x() = sin_b_half * cos(diff_a_g_half);
      q.y() = sin_b_half * sin(diff_a_g_half);
      q.z() = cos_b_half * sin(sum_a_g_half);
      q.w() = cos_b_half * cos(sum_a_g_half);
    }