Example #1
0
bool approxEqual(const MatrixWrapper::ColumnVector& a, const MatrixWrapper::ColumnVector& b, double epsilon)
{
  if (a.rows() != b.rows()) return false;

  for (unsigned int r=0; r<a.rows(); r++)
    if (!approxEqual(a(r+1), b(r+1),epsilon)) return false;

  return true;
}
    bool Quaternion_Wrapper::getEulerAngles(std::string axes, MatrixWrapper::ColumnVector& output) {
        bool ret = false;
        //  Normalize quaternion
        MyQuaternion& quat = (*(MyQuaternion*)this);
        quat.normalize();
        if (!(output.rows() == 3))
            ret = false;
        else {
            if (!axes.compare("xyz")) {
                double r = quat(1);
                double x = quat(2);
                double y = quat(3);
                double z = quat(4);
                output(1) = atan2(2*y*z + 2*x*r, 1 - 2*pow(x,2) - 2*pow(y,2));
                output(2) = asin(-2*x*z + 2*y*r);
                output(3) = atan2(2*x*y + 2*z*r, 1 - 2*pow(y,2) - 2*pow(z,2));
                // NOTE Reference http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
//                 output(1) = atan2(-2*x*y - 2*z*r, 1 - 2*pow(y,2) - 2*pow(z,2));
//                 output(2) = asin(-2*x*z + 2*y*r);
//                 output(3) = atan2(-2*z*y + 2*x*r, 1 - 2*pow(x,2) - 2*pow(y,2));
                ret = true;
            } else
                std::cout << "[quaternion_wrapper.cpp] Computation for axis order " << axes << " has not been implemented yet" <<  std::endl;
        }
        return ret;
    }
Example #3
0
void DetectorFilter::composeTransform(const MatrixWrapper::ColumnVector& vector,
                                      geometry_msgs::PoseWithCovarianceStamped& pose)
{
  assert(vector.rows() == 6);

  // construct kdl rotation from vector (x, y, z, Rx, Ry, Rz), and extract quaternion
  KDL::Rotation rot = KDL::Rotation::RPY(vector(4), vector(5), vector(6));
  rot.GetQuaternion(pose.pose.pose.orientation.x, pose.pose.pose.orientation.y,
                    pose.pose.pose.orientation.z, pose.pose.pose.orientation.w);

  pose.pose.pose.position.x = vector(1);
  pose.pose.pose.position.y = vector(2);
  pose.pose.pose.position.z = vector(3);
};
Example #4
0
void DetectorFilter::decomposeTransform(const geometry_msgs::PoseWithCovarianceStamped& pose,
                                        MatrixWrapper::ColumnVector& vector)
{
  assert(vector.rows() == 6);

  // construct kdl rotation from quaternion, and extract RPY
  KDL::Rotation rot = KDL::Rotation::Quaternion(pose.pose.pose.orientation.x,
                                                pose.pose.pose.orientation.y,
                                                pose.pose.pose.orientation.z,
                                                pose.pose.pose.orientation.w);
  rot.GetRPY(vector(4), vector(5), vector(6));

  vector(1) = pose.pose.pose.position.x;
  vector(2) = pose.pose.pose.position.y;
  vector(3) = pose.pose.pose.position.z;
};
void BeaconKFNode::beaconCallback( geometry_msgs::PoseWithCovarianceStampedConstPtr msg )
{
    tf::StampedTransform T_beacon_to_camera;
    tf::poseMsgToTF( msg->pose.pose, T_beacon_to_camera);
    ros::Time beacon_stamp = msg->header.stamp;
    T_beacon_to_camera.stamp_ = beacon_stamp;
    _camera_frame_id = msg->header.frame_id;
    
    //measured map->odom transform
    tf::Transform T_measured_mto;
    
    try {
        _tf.waitForTransform("base_link", _camera_frame_id,
                             beacon_stamp, ros::Duration(1.0));
        
        tf::StampedTransform T_camera_to_base;
        _tf.lookupTransform("base_link", _camera_frame_id, beacon_stamp, T_camera_to_base);
    
        _tf.waitForTransform(_odometry_frame, "base_link",
                             beacon_stamp, ros::Duration(1.0));
    
        tf::StampedTransform T_base_to_odom;
        _tf.lookupTransform(_odometry_frame, "base_link", beacon_stamp, T_base_to_odom);            
                
        //this is in the reverse of the normal xform direction, for the error xform calc
        //this is a static xform from beacon_finder, timestamp isn't important
        tf::StampedTransform T_map_to_beacon;
        _tf.lookupTransform("beacon", _world_fixed_frame, ros::Time(0), T_map_to_beacon);
                
        //This calculates the correction thusly:
        //odom->base->camera->measured-beacon * beacon->map
        T_measured_mto = T_base_to_odom*T_camera_to_base*T_beacon_to_camera*T_map_to_beacon;
        T_measured_mto = T_measured_mto.inverse();
        
        //broadcast this T_map_to odom from real map
        tf::StampedTransform test_map_to_odom(T_measured_mto,
                                              msg->header.stamp,
                                              _world_fixed_frame,
                                              "beacon_localizer_T");
        _tf_broadcast.sendTransform( test_map_to_odom );
    }
    catch (tf::TransformException &ex) {
         ROS_ERROR("BEACON_LOCALIZER transform lookup failure: %s", ex.what());
         return;
    }

    MatrixWrapper::ColumnVector measurement(3);

    measurement(1) = T_measured_mto.getOrigin()[0];
    measurement(2) = T_measured_mto.getOrigin()[1];
    measurement(3) = tf::getYaw( T_measured_mto.getRotation() );
    ROS_DEBUG_STREAM("BEACON LOCALIZER measurement: " << measurement.transpose() );

    MatrixWrapper::ColumnVector state =_filter->PostGet()->ExpectedValueGet();
    MatrixWrapper::ColumnVector err = (state-measurement);
    double distance = sqrt(err.transpose()*err);

    MatrixWrapper::SymmetricMatrix cov(6);
    cov(1,1) = msg->pose.covariance[0]; cov(1,2) = msg->pose.covariance[1]; cov(1,3) = msg->pose.covariance[2]; cov(1,4) = msg->pose.covariance[3]; cov(1,5) = msg->pose.covariance[4]; cov(1,6) = msg->pose.covariance[5];
    cov(2,1) = msg->pose.covariance[6]; cov(2,2) = msg->pose.covariance[7]; cov(2,3) = msg->pose.covariance[8]; cov(2,4) = msg->pose.covariance[9]; cov(2,5) = msg->pose.covariance[10];cov(2,6) = msg->pose.covariance[11];
    cov(3,1) = msg->pose.covariance[12];cov(3,2) = msg->pose.covariance[13];cov(3,3) = msg->pose.covariance[14];cov(3,4) = msg->pose.covariance[15];cov(3,5) = msg->pose.covariance[16];cov(3,6) = msg->pose.covariance[17];
    cov(4,1) = msg->pose.covariance[18];cov(4,2) = msg->pose.covariance[19];cov(4,3) = msg->pose.covariance[20];cov(4,4) = msg->pose.covariance[21];cov(4,5) = msg->pose.covariance[22];cov(4,6) = msg->pose.covariance[23];
    cov(5,1) = msg->pose.covariance[24];cov(5,2) = msg->pose.covariance[25];cov(5,3) = msg->pose.covariance[26];cov(5,4) = msg->pose.covariance[27];cov(5,5) = msg->pose.covariance[28];cov(5,6) = msg->pose.covariance[29];
    cov(6,1) = msg->pose.covariance[30];cov(6,2) = msg->pose.covariance[31];cov(6,3) = msg->pose.covariance[32];cov(6,4) = msg->pose.covariance[33];cov(6,5) = msg->pose.covariance[34];cov(6,6) = msg->pose.covariance[35];

    /*
       tf::Matrix3x3 pointDevMat(
       sqrt(cov(1,1)),              0,             0,
       0.0, sqrt(cov(2,2)),             0,
       0.0,            0.0, sqrt(cov(3,3)));
       ROS_INFO("Point deviation");
       ROS_INFO("[ %f, %f, %f ]", pointDevMat[0][0], pointDevMat[0][1], pointDevMat[0][2]);
       ROS_INFO("[ %f, %f, %f ]", pointDevMat[1][0], pointDevMat[1][1], pointDevMat[1][2]);
       ROS_INFO("[ %f, %f, %f ]", pointDevMat[2][0], pointDevMat[2][1], pointDevMat[2][2]);

       tf::Matrix3x3 rpointDev = _T_odom.getBasis()*T_camera_to_base.getBasis()*pointDevMat*T_map_to_beacon.getBasis();
       tf::Matrix3x3 rpointCov = rpointDev*rpointDev;
       ROS_INFO("rPoint covariance");
       ROS_INFO("[ %f, %f, %f ]", rpointCov[0][0], rpointCov[0][1], rpointCov[0][2]);
       ROS_INFO("[ %f, %f, %f ]", rpointCov[1][0], rpointCov[1][1], rpointCov[1][2]);
       ROS_INFO("[ %f, %f, %f ]", rpointCov[2][0], rpointCov[2][1], rpointCov[2][2]);
       */

    MatrixWrapper::SymmetricMatrix measNoiseCovariance(3);
    //measNoiseCovariance(1,1) = rpointCov[0][0]; measNoiseCovariance(1,2) = rpointCov[0][1]; measNoiseCovariance(1,3) = 0;
    //measNoiseCovariance(2,1) = rpointCov[1][0]; measNoiseCovariance(2,2) = rpointCov[1][1]; measNoiseCovariance(2,3) = 0;
    //measNoiseCovariance(3,1) = 0;               measNoiseCovariance(3,2) = 0;               measNoiseCovariance(3,3) = cov(5,5);
    measNoiseCovariance(1,1) = cov(3,3); measNoiseCovariance(1,2) =      0.0; measNoiseCovariance(1,3) = 0.0;
    measNoiseCovariance(2,1) =      0.0; measNoiseCovariance(2,2) = cov(3,3); measNoiseCovariance(2,3) = 0.0;
    measNoiseCovariance(3,1) =      0.0; measNoiseCovariance(3,2) =      0.0; measNoiseCovariance(3,3) = cov(5,5);
    ROS_DEBUG_STREAM("BEACON LOCALIZER measurement noise covariance: " << measNoiseCovariance);
    MatrixWrapper::ColumnVector measNoiseMu(3); // zeros
    measNoiseMu(1) = 0.0;
    measNoiseMu(2) = 0.0;
    measNoiseMu(3) = 0.0;
    BFL::Gaussian measUncertainty(measNoiseMu, measNoiseCovariance);

    MatrixWrapper::Matrix measH(3,3);
    measH(1,1) = 1.0; measH(1,2) = 0.0; measH(1,3) = 0.0;
    measH(2,1) = 0.0; measH(2,2) = 1.0; measH(2,3) = 0.0;
    measH(3,1) = 0.0; measH(3,2) = 0.0; measH(3,3) = 1.0;

    BFL::LinearAnalyticConditionalGaussian beaconMeasurementPdf(measH, measUncertainty);

    BFL::LinearAnalyticMeasurementModelGaussianUncertainty beaconMeasurementModel(&beaconMeasurementPdf);

    _filter->Update(_system_model, &beaconMeasurementModel, measurement);

    ROS_DEBUG_STREAM("BEACON LOCALIZER State: " << _filter->PostGet()->ExpectedValueGet().transpose() );
    ROS_DEBUG_STREAM("BEACON LOCALIZER Covariance: " << _filter->PostGet()->CovarianceGet() );
}
  void
  SRIteratedExtendedKalmanFilter::MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,const MatrixWrapper::ColumnVector& z,const MatrixWrapper::ColumnVector& s)
  {

    MatrixWrapper::Matrix invS(z.rows(),z.rows());
    MatrixWrapper::Matrix Sr(z.rows(),z.rows());
    MatrixWrapper::Matrix K_i(_post->CovarianceGet().rows(),z.rows());

    MatrixWrapper::ColumnVector    x_k = _post->ExpectedValueGet();
    MatrixWrapper::SymmetricMatrix P_k = _post->CovarianceGet();
    MatrixWrapper::ColumnVector    x_i = _post->ExpectedValueGet();

    MatrixWrapper::Matrix       	H_i;    	MatrixWrapper::SymmetricMatrix  	 R_i;
    MatrixWrapper::Matrix  		R_vf;		MatrixWrapper::Matrix       		 SR_vf;
    MatrixWrapper::ColumnVector     	Z_i;
    MatrixWrapper::Matrix 	   	U; 		MatrixWrapper::ColumnVector              V;   	MatrixWrapper::Matrix             W;
    MatrixWrapper::Matrix         	JP1;      	int change;


    Matrix diag(JP.rows(),JP.columns());
    Matrix invdiag(JP.rows(),JP.columns());
    diag=0;invdiag=0;change=0;
    V=0;U=0;W=0;

    // matrix determining the numerical limitations of covariance matrix:
    for(unsigned int j=1;j<JP.rows()+1;j++){diag(j,j)=100; invdiag(j,j)=0.01;}


    for (unsigned int i=1; i<nr_iterations+1; i++)
      {
	x_i = _post->ExpectedValueGet();

	H_i  = ((LinearAnalyticMeas_Implicit*)measmodel)->df_dxGet(s,x_i);
	Z_i  = ((LinearAnalyticMeas_Implicit*)measmodel)->ExpectedValueGet() + ( H_i * (x_k - x_i) );

	R_i  = ((LinearAnalyticMeas_Implicit*)measmodel)->CovarianceGet();
	SR_vf  = ((LinearAnalyticMeas_Implicit*)measmodel)->SRCovariance();

	// check two different types of Kalman filters:
	 if(((LinearAnalyticMeas_Implicit*)measmodel)->Is_Identity()==1)
         {
                     R_vf = SR_vf.transpose();
         }
         else
         {
                     R_i.cholesky_semidefinite(R_vf);
                     R_vf = R_vf.transpose();
         }

	// numerical limitations
	// The singular values of the Square root covariance matrix are limited the the value of 10e-4
	// because of numerical stabilisation of the Kalman filter algorithm.
	 JP.SVD(V,U,W);
	 MatrixWrapper::Matrix V_matrix(U.columns(),W.columns());
	for(unsigned int k=1;k<JP.rows()+1;k++)
	{
   	        V_matrix(k,k) = V(k);
		V(k)=max(V(k),1.0/(Numerical_Limitation));
		if(V(k)==1/(Numerical_Limitation)){change=1;}
	}
	if(change==1)
	{
		JP   = U*V_matrix*(W.transpose());
	}

	// end limitations

	CalculateMatrix(H_i,  R_i , invS , K_i , Sr );

	CalculateMean(x_k, z, Z_i , K_i);

	if (i==nr_iterations)
	{
		CalculateCovariance( R_vf, H_i, invS, Sr );
	}

    }
  }