Exemplo n.º 1
0
// 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;
}
Exemplo n.º 2
0
		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;
    }
Exemplo n.º 5
0
 Transform3D Transform3D::createScale(const arma::vec3& v) {
     Transform3D transform;
     transform.rotation() = arma::diagmat(v);
     return transform;
 }
Exemplo n.º 6
0
 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());
 }