TEST(TimeCache, AngularInterpolation) { uint64_t runs = 100; double epsilon = 1e-6; seed_rand(); tf::TimeCache cache; std::vector<double> yawvalues(2); std::vector<double> pitchvalues(2); std::vector<double> rollvalues(2); uint64_t offset = 200; std::vector<btQuaternion> quats(2); StampedTransform t; t.setIdentity(); for ( uint64_t i = 1; i < runs ; i++ ) { for (uint64_t step = 0; step < 2 ; step++) { yawvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX / 100.0; pitchvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; rollvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]); t.setRotation(quats[step]); t.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1 TransformStorage stor(t, 3, 0); cache.insertData(stor); } TransformStorage out; for (int pos = 0; pos < 100 ; pos ++) { cache.getData(ros::Time().fromNSec(offset + pos), out); //get the transform for the position btQuaternion quat = out.rotation_; //get the quaternion out of the transform //Generate a ground truth quaternion directly calling slerp btQuaternion ground_truth = quats[0].slerp(quats[1], pos/100.0); //Make sure the transformed one and the direct call match EXPECT_NEAR(0, angle(ground_truth, quat), epsilon); } cache.clearList(); } }
void Transformer::lookupTransform(const std::string& target_frame, const std::string& source_frame, const Time& time, StampedTransform& transform) const { std::string mapped_tgt = assert_resolved(tf_prefix_, target_frame); std::string mapped_src = assert_resolved(tf_prefix_, source_frame); if (mapped_tgt == mapped_src) { transform.setIdentity(); transform.child_frame_id_ = mapped_src; transform.frame_id_ = mapped_tgt; transform.stamp_ = now(); return; } boost::recursive_mutex::scoped_lock lock(frame_mutex_); CompactFrameID target_id = lookupFrameNumber(mapped_tgt); CompactFrameID source_id = lookupFrameNumber(mapped_src); std::string error_string; TransformAccum accum; int retval = walkToTopParent(accum, time, target_id, source_id, &error_string); if (retval != NO_ERROR) { switch (retval) { case CONNECTIVITY_ERROR: throw ConnectivityException(error_string); case EXTRAPOLATION_ERROR: throw ExtrapolationException(error_string); case LOOKUP_ERROR: throw LookupException(error_string); default: fprintf(stderr,"Unknown error code: %d\n", retval); break; } } transform.setOrigin(accum.result_vec); transform.setRotation(accum.result_quat); transform.child_frame_id_ = mapped_src; transform.frame_id_ = mapped_tgt; transform.stamp_ = accum.time; };
void currentSweepHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudIn) { pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloud(new pcl::PointCloud<pcl::PointXYZHSV>()); pcl::fromROSMsg(*laserCloudIn, *laserCloud); gLaserOdometry->addCurrentSweep(laserCloud, laserCloudIn->header.stamp.toSec()); // Translation and Rotation of the calculated Odometry double tx = 0; double ty = 0; double tz = 0; double rx = 0; double ry = 0; double rz = 0; // Publish the calculated odometry via TF StampedTransform laserOdometry; laserOdometry.setOrigin(Vector3(tx, ty, tz)); laserOdometry.setRotation(createQuaternionFromRPY(rz, -rx, -ry)); laserOdometry.frame_id_ = "/camera_init"; laserOdometry.child_frame_id_ = "/camera"; laserOdometry.stamp_ = laserCloudIn->header.stamp; tfBroadcaster->sendTransform(laserOdometry); }