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