std::vector<Matrix> Block::eigh(cflag tp)const{
std::vector<Matrix> outs;
try{
checkUni10TypeError(tp);
if(!(Rnum == Cnum)){
std::ostringstream err;
err<<"Cannot perform eigenvalue decomposition on a non-square matrix.";
throw std::runtime_error(exception_msg(err.str()));
}
if(diag){
std::ostringstream err;
err<<"Cannot perform eigenvalue decomposition on a diagonal matrix. Need not to do so.";
throw std::runtime_error(exception_msg(err.str()));
}
//GPU_NOT_READY
outs.push_back(Matrix(CTYPE, Rnum, Cnum, true, ongpu));
outs.push_back(Matrix(CTYPE, Rnum, Cnum, false, ongpu));
Matrix Eig(RTYPE, Rnum, Cnum, true, ongpu);
eigSyDecompose(cm_elem, Rnum, Eig.m_elem, outs[1].cm_elem, ongpu);
outs[0] = Eig;
}
catch(const std::exception& e){
propogate_exception(e, "In function Matrix::eigh(uni10::cflag ):");
}
return outs;
}
std::vector<Matrix> Block::ql(cflag tp)const{
std::vector<Matrix> outs;
try{
checkUni10TypeError(tp);
if(Rnum < Cnum){
std::ostringstream err;
err<<"Cannot perform QL decomposition when Rnum < Cnum. Nothing to do.";
throw std::runtime_error(exception_msg(err.str()));
}
outs.push_back(Matrix(CTYPE, Rnum, Cnum, false, ongpu));
outs.push_back(Matrix(CTYPE, Cnum, Cnum, false, ongpu));
if(!diag){
matrixQL(cm_elem, Rnum, Cnum, outs[0].cm_elem, outs[1].cm_elem, ongpu);
}else{
size_t min = std::min(Rnum, Cnum);
Complex* tmpC = (Complex*)calloc(min*min , sizeof(Complex));
for(size_t i = 0; i < min; i++)
tmpC[i*min+i] = cm_elem[i];
matrixQL(tmpC, min, min, outs[0].cm_elem, outs[1].cm_elem, ongpu);
free(tmpC);
}
}
catch(const std::exception& e){
propogate_exception(e, "In function Block::ql(uni10::cflag ):");
}
return outs;
}
void Block::save(cflag tp, const std::string& fname)const{
try{
checkUni10TypeError(tp);
FILE *fp = fopen(fname.c_str(), "w");
if(!(fp != NULL)){
std::ostringstream err;
err<<"Error in writing to file '"<<fname<<"'.";
throw std::runtime_error(exception_msg(err.str()));
}
fwrite(&r_flag, sizeof(r_flag), 1, fp);
fwrite(&c_flag, sizeof(c_flag), 1, fp);
fwrite(&Rnum, sizeof(Rnum), 1, fp);
fwrite(&Cnum, sizeof(Cnum), 1, fp);
fwrite(&diag, sizeof(diag), 1, fp);
fwrite(&ongpu, sizeof(ongpu), 1, fp);
Complex* elem = cm_elem;
if(ongpu){
elem = (Complex*)malloc(elemNum() * sizeof(Complex));
elemCopy(elem, cm_elem, Rnum, Cnum, false, ongpu, diag);
}
fwrite(elem, sizeof(Complex), elemNum(), fp);
if(ongpu)
free(elem);
fclose(fp);
}
catch(const std::exception& e){
propogate_exception(e, "In function Block::save(uni10::cflag, std::string&):");
}
}
std::vector<Matrix> Block::lq(rflag tp)const{
std::vector<Matrix> outs;
try{
checkUni10TypeError(tp);
if(Rnum > Cnum){
std::ostringstream err;
err<<"Cannot perform LQ decomposition when Rnum > Cnum. Nothing to do.";
throw std::runtime_error(exception_msg(err.str()));
}
outs.push_back(Matrix(RTYPE, Rnum, Rnum, false, ongpu));
outs.push_back(Matrix(RTYPE, Rnum, Cnum, false, ongpu));
if(!diag){
matrixLQ(m_elem, Rnum, Cnum, outs[1].m_elem, outs[0].m_elem, ongpu);
}else{
size_t min = std::min(Rnum, Cnum);
Real* tmpR = (Real*)calloc(min*min, sizeof(Real));
for(size_t i = 0; i < min; i++)
tmpR[i*min+i] = m_elem[i];
matrixLQ(tmpR, min, min, outs[1].m_elem, outs[0].m_elem, ongpu);
free(tmpR);
}
}
catch(const std::exception& e){
propogate_exception(e, "In function Block::lq(uni10::rflag ):");
}
return outs;
}
Matrix::Matrix(cflag tp, const std::string& fname){
try{
checkUni10TypeError(tp);
FILE *fp = fopen(fname.c_str(), "r");
if(!(fp != NULL)){
std::ostringstream err;
err<<"Error in opening file '" << fname <<"'.";
throw std::runtime_error(exception_msg(err.str()));
}
fread(&r_flag, sizeof(r_flag), 1, fp);
fread(&c_flag, sizeof(c_flag), 1, fp);
fread(&Rnum, sizeof(Rnum), 1, fp);
fread(&Cnum, sizeof(Cnum), 1, fp);
fread(&diag, sizeof(diag), 1, fp);
fread(&ongpu, sizeof(ongpu), 1, fp);
init(tp, ongpu);
if(elemNum())
elemBzero(cm_elem, elemNum() * sizeof(Complex), ongpu);
Complex* elem = cm_elem;
if(ongpu)
elem = (Complex*)malloc(elemNum() * sizeof(Complex));
fread(elem, sizeof(Complex), elemNum(), fp);
if(ongpu){
elemCopy(cm_elem, elem, elemNum() * sizeof(Complex), ongpu, false);
free(elem);
}
fclose(fp);
}
catch(const std::exception& e){
propogate_exception(e, "In constructor Matrix::Matrix(uni10::cflag, std::string& )");
}
}
Complex Block::operator()(size_t idx)const{
try{
if(!(idx < elemNum())){
std::ostringstream err;
err<<"Index exceeds the number of elements("<<elemNum()<<").";
throw std::runtime_error(exception_msg(err.str()));
}
if(typeID() == 0 || typeID() == 1){
std::ostringstream err;
err<<"This matrix is EMPTY or REAL. If it's REAL, please use operator[] instead of operator().";
throw std::runtime_error(exception_msg(err.str()));
}
if(typeID() == 2)
return getElemAt(idx, cm_elem, ongpu), 0;
}
catch(const std::exception& e){
propogate_exception(e, "In function Block::operator()(size_t):");
}
return Complex();
};
void Matrix::setElem(const std::vector<Complex>& elem, bool src_ongpu){
try{
if(diag == false && Rnum*Cnum != elem.size() ){
std::ostringstream err;
err<<"Number of the input elements is: " << elem.size() <<", and it doesn't match to the size of matrix: "\
<< Rnum*Cnum << std::endl <<"In the file Block.cpp, line(" << __LINE__ << ")";
throw std::runtime_error(exception_msg(err.str()));
}
if(diag == true && std::min(Rnum, Cnum) != elem.size()){
std::ostringstream err;
err<<"Number of the input elements is: " << elem.size() <<", and it doesn't match to the min(Rnum, Cnum) of matrix: "\
<< std::min(Rnum, Cnum) << std::endl <<"In the file Block.cpp, line(" << __LINE__ << ")";
throw std::runtime_error(exception_msg(err.str()));
}
setElem(&elem[0], src_ongpu);
}
catch(const std::exception& e){
propogate_exception(e, "In function Matrix::setElem(std::vector<Complex>&, bool=false):");
}
}
Qnum::Qnum(int _U1, parityType _prt): m_U1(_U1), m_prt(_prt), m_prtF(PRTF_EVEN){
try{
if(!(m_U1 < U1_UPB && m_U1 > U1_LOB)){
std::ostringstream err;
err<<"U1 is out of range. "<<U1_LOB<<" < U1 < "<<U1_UPB<<".";
throw std::runtime_error(exception_msg(err.str()));
}
}
catch(const std::exception& e){
propogate_exception(e, "In constructor Qnum::Qnum(int, parityType):");
}
}
Matrix::Matrix(size_t _Rnum, size_t _Cnum, const std::vector<Complex>& _elem, bool _diag, bool _ongpu, bool src_ongpu): Block(CTYPE, _Rnum, _Cnum, _diag){
try{
ongpu = _ongpu;
if(_diag == false && _Rnum*_Cnum != _elem.size() ){
std::ostringstream err;
err<<"Number of the input elements is: " << _elem.size() <<", and it doesn't match to the size of matrix: "\
<< _Rnum*_Cnum << std::endl <<"In the file Block.cpp, line(" << __LINE__ << ")";
throw std::runtime_error(exception_msg(err.str()));
}
if( _diag == true && std::min(_Rnum, _Cnum) != _elem.size()){
std::ostringstream err;
err<<"Number of the input elements is: " << _elem.size() <<", and it doesn't match to the min(Rnum, Cnum) of matrix: "\
<< std::min(_Rnum, _Cnum) << std::endl <<"In the file Block.cpp, line(" << __LINE__ << ")";
throw std::runtime_error(exception_msg(err.str()));
}
init(&_elem[0], src_ongpu);
}
catch(const std::exception& e){
propogate_exception(e, "In constructor Matrix::Matrix(size_t, size_t, std::vector<Complex>&, bool=false):");
}
}
Real* Block::getElem(rflag tp)const{
throwTypeError(tp);
try{
if(typeID() == 2){
std::ostringstream err;
err<<"This matrix is COMPLEX. Please use getElem(uni10::cflag ) instead.";
throw std::runtime_error(exception_msg(err.str()));
}
return m_elem;
}
catch(const std::exception& e){
propogate_exception(e, "In function Block::getElem(uni10::rflag ):");
}
return m_elem;
}
Complex* Block::getElem(cflag tp)const{
checkUni10TypeError(tp);
try{
if(typeID() == 1){
std::ostringstream err;
err<<"This matrix is REAL. Please use getElem() or getElem(uni10::rflag ) instead.";
throw std::runtime_error(exception_msg(err.str()));
}
return cm_elem;
}
catch(const std::exception& e){
propogate_exception(e, "In function Block::getElem(uni10::cflag ):");
}
return cm_elem;
}
Complex Block::at(cflag tp, size_t r, size_t c)const{
try{
checkUni10TypeError(tp);
if(!((r < Rnum) && (c < Cnum))){
std::ostringstream err;
err<<"The input indices are out of range.";
throw std::runtime_error(exception_msg(err.str()));
}
if(diag){
if(!(r == c && r < elemNum())){
std::ostringstream err;
err<<"The matrix is diagonal, there is no off-diagonal element.";
throw std::runtime_error(exception_msg(err.str()));
}
return getElemAt(r, cm_elem, ongpu);
}
else
return getElemAt(r * Cnum + c, cm_elem, ongpu);
}
catch(const std::exception& e){
propogate_exception(e, "In function Block::at(uni10::cflag, size_t, size_t):");
}
return Complex();
}
Real Block::norm(cflag tp)const{
try{
checkUni10TypeError(tp);
if(typeID() == 0){
std::ostringstream err;
err<<"This matirx is empty" << std::endl << "In the file Block.cpp, line(" << __LINE__ << ")";
throw std::runtime_error(exception_msg(err.str()));
}
return vectorNorm(cm_elem, elemNum(), 1, ongpu);
}
catch(const std::exception& e){
propogate_exception(e, "In function Matrix::norm(uni10::cflag ):");
}
return 0;
}
Complex& Matrix::at(cflag tp, size_t idx){
try{
checkUni10TypeError(tp);
if(!(idx < elemNum())){
std::ostringstream err;
err<<"Index exceeds the number of the matrix elements("<<elemNum()<<").";
throw std::runtime_error(exception_msg(err.str()));
}
cm_elem = (Complex*)mvCPU(cm_elem, elemNum() * sizeof(Complex), ongpu);
return cm_elem[idx];
}
catch(const std::exception& e){
propogate_exception(e, "In function Matrix::at(uni10::cflag , size_t ):");
return cm_elem[0];
}
}
Complex Block::trace(cflag tp)const{
try{
checkUni10TypeError(tp);
if(!(Rnum == Cnum)){
std::ostringstream err;
err<<"Cannot perform trace on a non-square matrix.";
throw std::runtime_error(exception_msg(err.str()));
}
if(diag)
return vectorSum(cm_elem, elemNum(), 1, ongpu);
else
return vectorSum(cm_elem, Cnum, Cnum + 1, ongpu);
}catch(const std::exception& e){
propogate_exception(e, "In function Matrix::trace(uni10::cflag ):");
return 0;
}
}
void UAURSmoothingFilterKalman::Measurement(FTransform const & MeasuredTransform)
{
try
{
FVector translation = MeasuredTransform.GetTranslation();
FString msg = "In: " + translation.ToString();
//FQuat rotation = MeasuredTransform.GetRotation();
TransformAsMat.at<float>(0) = translation.X;
TransformAsMat.at<float>(1) = translation.Y;
TransformAsMat.at<float>(2) = translation.Z;
//TransformAsMat.at<float>(3) = rotation.X * rotation.W;
//TransformAsMat.at<float>(4) = rotation.Y * rotation.W;
//TransformAsMat.at<float>(5) = rotation.Z * rotation.W;
cv::Mat FilterResult = Filter.predict();
Filter.correct(TransformAsMat);
translation.Set(FilterResult.at<float>(0), FilterResult.at<float>(1), FilterResult.at<float>(2));
msg += "\nOut: " + translation.ToString();
UE_LOG(LogAUR, Log, TEXT("%s"), *msg);
//FVector rot_axis(FilterResult.at<float>(3), FilterResult.at<float>(4), FilterResult.at<float>(5));
//float rot_magnitude = rot_axis.Size();
//rot_axis.Normalize();
//rotation.X = rot_axis.X;
//rotation.Y = rot_axis.Y;
//rotation.Z = rot_axis.Z;
//rotation.W = rot_magnitude;
CurrentTransform.SetComponents(MeasuredTransform.GetRotation(), translation, FVector(1, 1, 1));
}
catch (cv::Exception& e)
{
FString exception_msg(e.what());
UE_LOG(LogAUR, Error, TEXT("Kalman: %s"), *exception_msg)
CurrentTransform = MeasuredTransform;
}
}
Matrix Block::inverse(cflag tp)const{
try{
checkUni10TypeError(tp);
if(!(Rnum == Cnum)){
std::ostringstream err;
err<<"Cannot perform inversion on a non-square matrix.";
throw std::runtime_error(exception_msg(err.str()));
}
Matrix invM(*this);
assert(ongpu == invM.isOngpu());
matrixInv(invM.cm_elem, Rnum, invM.diag, invM.ongpu);
return invM;
}
catch(const std::exception& e){
propogate_exception(e, "In function Matrix::inverse(uni10::cflag ):");
return Matrix();
}
}