Matrix4d Transformation::getInvTransform() { if ( mDirty ) { computeTransform(); mDirty = false; } return mTransform.inverse(); }
Matrix4d TrfmRotateExpMap::getInvTransform(){ if(mDirty){ computeTransform(); mDirty=false; } return mTransform.transpose(); }
Matrix4d Transformation::getTransform() { if ( mDirty ) { computeTransform(); mDirty = false; } return mTransform; }
void NavSatTransform::run() { ros::Time::init(); double frequency = 10; ros::NodeHandle nh; ros::NodeHandle nhPriv("~"); // Subscribe to the messages we need ros::Subscriber odomSub = nh.subscribe("odometry/filtered", 1, &NavSatTransform::odomCallback, this); ros::Subscriber gpsSub = nh.subscribe("gps/fix", 1, &NavSatTransform::gpsFixCallback, this); ros::Subscriber imuSub = nh.subscribe("imu/data", 1, &NavSatTransform::imuCallback, this); // Load the parameters we need nhPriv.getParam("magnetic_declination_radians", magneticDeclination_); nhPriv.getParam("yaw_offset", yawOffset_); nhPriv.param("broadcast_utm_transform", broadcastUtmTransform_, false); nhPriv.param("zero_altitude", zeroAltitude_, false); nhPriv.param("publish_filtered_gps", publishGps_, false); nhPriv.param("frequency", frequency, 10.0); ros::Publisher gpsOdomPub = nh.advertise<nav_msgs::Odometry>("odometry/gps", 10); ros::Publisher filteredGpsPub; if(publishGps_) { filteredGpsPub = nh.advertise<sensor_msgs::NavSatFix>("gps/filtered", 10); } tf::TransformBroadcaster utmBroadcaster; tf::StampedTransform utmTransformStamped; utmTransformStamped.child_frame_id_ = "utm"; ros::Rate rate(frequency); while(ros::ok()) { ros::spinOnce(); if(!transformGood_) { computeTransform(); if(transformGood_) { // Once we have the transform, we don't need the IMU imuSub.shutdown(); } } else { nav_msgs::Odometry gpsOdom; if(prepareGpsOdometry(gpsOdom)) { gpsOdomPub.publish(gpsOdom); } if(publishGps_) { sensor_msgs::NavSatFix odomGps; if(prepareFilteredGps(odomGps)) { filteredGpsPub.publish(odomGps); } } // Send out the UTM transform in case anyone // else would like to use it. if(transformGood_ && broadcastUtmTransform_) { utmTransformStamped.setData(utmWorldTransform_); utmTransformStamped.frame_id_ = worldFrameId_; utmTransformStamped.stamp_ = ros::Time::now(); utmBroadcaster.sendTransform(utmTransformStamped); } } rate.sleep(); } }
CPerspectiveCamera::CPerspectiveCamera(const CPoint& position, const CPoint& target, const F& fov, unsigned int width, unsigned int height, E_IMAGE_TYPE type) : ICamera(position, target, width, height, type), mFOV(fov) { computeTransform(); }
void CPerspectiveCamera::SetTarget(const CPoint& target) { mTarget = target; computeTransform(); }
void CPerspectiveCamera::SetPosition(const CPoint& position) { mPosition = position; computeTransform(); }