// Qt Streams
QDebug operator<<(QDebug dbg, const Transform3D &transform)
{
dbg << "Transform3D\n{\n";
dbg << "Position: <" << transform.translation().x() << ", " << transform.translation().y() << ", " << transform.translation().z() << ">\n";
dbg << "Scale: <" << transform.scale().x() << ", " << transform.scale().y() << ", " << transform.scale().z() << ">\n";
dbg << "Rotation: <" << transform.rotation().x() << ", " << transform.rotation().y() << ", " << transform.rotation().z() << " | " << transform.rotation().scalar() << ">\n}";
return dbg;
}
void Correlator::addData(MocapStream::RigidBodyID id1, Transform3D T1, MocapStream::RigidBodyID id2, Transform3D T2){
//Generate key for the map of correlationStats
std::pair<MocapStream::RigidBodyID, MocapStream::RigidBodyID> key = {id1,id2};
//Check if this hypothesis has been eliminated
if(eliminatedHypotheses.count(key) != 0) return;
if(recordedStates.count(key) == 0){
//Initialise if key missing. (true => calculate cov)
recordedStates[key] = StreamPair();
} else {
// std::cout << "number of samples = " << recordedStates[key].first.size() << std::endl;
// std::cout << "diff1 = " << diff1 << std::endl;
// if( id1 == 1 && id2 == 18) std::cout << "T2 = " << T2 << std::endl;
// std::cout << "diff2 = " << diff2 << std::endl;
// std::cout << "T2 = " << T2 << std::endl;
//If we have no recorded states yet, or the new states differ significantly from the previous, add the new states
if( stateIsNew(T1, recordedStates[key].first)
|| stateIsNew(T2, recordedStates[key].second) )
{
//Check if bad sample (for the particular solver we are using):
Rotation3D R1 = T1.rotation();
UnitQuaternion q1(R1);
Rotation3D R2 = T2.rotation();
UnitQuaternion q2(R2);
if(q1.kW() == 0 || q2.kW() == 0) return;
//Check det
if(std::fabs(arma::det(R1) - 1) > 0.1){
std::cout << __FILE__ << " : Det R1 = " << arma::det(R1) << std::endl;
}
if(std::fabs(arma::det(R2) - 1) > 0.1){
std::cout << __FILE__ << " : Det R2 = " << arma::det(R2) << std::endl;
}
std::cout << "Recording Sample..." << std::endl;
if(recordedStates[key].first.size() >= number_of_samples){
recordedStates[key].first.erase(recordedStates[key].first.begin());
recordedStates[key].first.push_back(T1);
recordedStates[key].second.erase(recordedStates[key].second.begin());
recordedStates[key].second.push_back(T2);
//Now we are ready to compute
computableStreams.insert(key);
} else {
recordedStates[key].first.push_back(T1);
recordedStates[key].second.push_back(T2);
}
}
}
}
float Transform3D::norm(Transform3D T){
float pos_norm = arma::norm(T.translation());
UnitQuaternion q = UnitQuaternion(Rotation3D(T.submat(0,0,2,2)));
float angle = q.getAngle();
//TODO: how to weight these two?
return pos_norm + Rotation3D::norm(T.rotation());
}
Transform3D Transform3D::interpolate(Transform3D T1, Transform3D T2, float alpha){
Rotation3D r1 = T1.rotation();
UnitQuaternion q1 = UnitQuaternion(r1);
Rotation3D r2 = T2.rotation();
UnitQuaternion q2 = UnitQuaternion(r2);
arma::vec3 t1 = T1.translation();
arma::vec3 t2 = T2.translation();
UnitQuaternion qResult = q1.slerp(q2,alpha);
arma::vec3 tResult = alpha * (t2 - t1) + t1;
Transform3D TResult = Transform3D(Rotation3D(qResult));
TResult.translation() = tResult;
return TResult;
}
Transform3D Transform3D::createScale(const arma::vec3& v) {
Transform3D transform;
transform.rotation() = arma::diagmat(v);
return transform;
}
float Transform3D::norm(Transform3D T) {
float pos_norm = arma::norm(T.translation());
// return Rotation3D::norm(T.rotation());
// TODO: how to weight these two?
return pos_norm + Rotation3D::norm(T.rotation());
}
