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