void CalcObjectLocation::calculate() {
    CLOG(LDEBUG)<<"in calculate()";

    Types::HomogMatrix homogMatrix;
    vector<cv::Mat_<double> > rvec;
    vector<cv::Mat_<double> > tvec;
    vector<cv::Mat_<double> > axis;
    vector<double> fi;
    cv::Mat_<double> tvectemp;
    cv::Mat_<double> rotMatrix;
    rotMatrix = cv::Mat_<double>::zeros(3,3);
    tvectemp = cv::Mat_<double>::zeros(3,1);

    while (!in_homogMatrix.empty()) {
        cv::Mat_<double> rvectemp;
        homogMatrix=in_homogMatrix.read();

        if (homogMatrix.getElements() == Eigen::Matrix4f::Identity()) {
            continue;
        }

        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                rotMatrix(i,j)=homogMatrix.getElement(i, j);
            }
            tvectemp(i, 0) = homogMatrix.getElement(i, 3);
        }

        Rodrigues(rotMatrix, rvectemp);
        CLOG(LINFO) << rvectemp << "\n";
        rvec.push_back(rvectemp);
        tvec.push_back(tvectemp);
    }

    if (rvec.size() == 1) {
        out_homogMatrix.write(homogMatrix);
        return;
    }

    float fi_sum=0, fi_avg;
    cv::Mat_<double> axis_sum, axis_avg;
    cv::Mat_<double> rvec_avg;
    cv::Mat_<double> tvec_avg, tvec_sum;

    axis_sum = cv::Mat_<double>::zeros(3,1);
    tvec_sum = cv::Mat_<double>::zeros(3,1);

    for(int i = 0; i < rvec.size(); i++) {
        float fitmp = sqrt((pow(rvec.at(i)(0,0), 2) + pow(rvec.at(i)(1,0), 2)+pow(rvec.at(i)(2,0),2)));
        fi.push_back(fitmp);

        fi_sum+=fitmp;
        cv::Mat_<double> axistemp;
        axistemp.create(3,1);
        for(int k=0; k<3; k++) {
            axistemp(k,0)=rvec.at(i)(k,0)/fitmp;
        }
        axis.push_back(axistemp);
        axis_sum+=axistemp;
        tvec_sum+=tvec.at(i);
    }

    fi_avg = fi_sum/fi.size();
    axis_avg = axis_sum/axis.size();
    rvec_avg = axis_avg * fi_avg;
    tvec_avg = tvec_sum/tvec.size();

    Types::HomogMatrix hm;
    cv::Mat_<double> rottMatrix;
    Rodrigues(rvec_avg, rottMatrix);

    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            hm.setElement(i, j, rottMatrix(i, j));
            CLOG(LINFO) << hm.getElement(i, j) << "  ";
        }
        hm.setElement(i, 3, tvec_avg(i, 0));
        CLOG(LINFO) << hm.getElement(i, 3) << "\n";
    }
    out_homogMatrix.write(hm);
}
void CalcStatistics::calculate() {
	CLOG(LDEBUG)<<"in calculate()";

	Types::HomogMatrix homogMatrix;
	cv::Mat_<double> rvec;
	cv::Mat_<double> tvec;
	cv::Mat_<double> axis;
	cv::Mat_<double> rotMatrix;
	float fi;

	rotMatrix= cv::Mat_<double>::zeros(3,3);
	tvec = cv::Mat_<double>::zeros(3,1);
	axis = cv::Mat_<double>::zeros(3,1);

	//first homogMatrix on InputStream
	if(counter == 0) {
		homogMatrix = in_homogMatrix.read();
		for (int i = 0; i < 3; ++i) {
			for (int j = 0; j < 3; ++j) {
                rotMatrix(i,j)=homogMatrix.getElement(i, j);
			}
            tvec(i, 0) = homogMatrix.getElement(i, 3);
		}

		Rodrigues(rotMatrix, rvec);

		cumulatedHomogMatrix = homogMatrix;
		cumulatedTvec = tvec;
		cumulatedRvec = rvec;
		fi = sqrt((pow(rvec(0,0), 2) +
						pow(rvec(1,0), 2)+pow(rvec(2,0),2)));
		cumulatedFi = fi;
		for(int k=0;k<3;k++) {
			axis(k,0)=rvec(k,0)/fi;
		}

		cumulatedAxis = axis;
		counter=1;
		return;
	}

	homogMatrix=in_homogMatrix.read();

	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
            rotMatrix(i,j)=homogMatrix.getElement(i, j);
		}
        tvec(i, 0) = homogMatrix.getElement(i, 3);
	}

	Rodrigues(rotMatrix, rvec);

	fi = sqrt((pow(rvec(0,0), 2) + pow(rvec(1,0), 2)+pow(rvec(2,0),2)));

	for(int k=0;k<3;k++) {
			axis(k,0)=rvec(k,0)/fi;
	}
	cumulatedFi += fi;
	cumulatedTvec += tvec;
	cumulatedRvec += rvec;
	cumulatedAxis += axis;

	counter++;
	avgFi = cumulatedFi/counter;
	avgAxis = cumulatedAxis/counter;
	avgRvec = avgAxis * avgFi;
	avgTvec = cumulatedTvec/counter;

	Types::HomogMatrix hm;
	cv::Mat_<double> rottMatrix;
	Rodrigues(avgRvec, rottMatrix);

	CLOG(LINFO)<<"Uśredniona macierz z "<<counter<<" macierzy \n";
	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
            hm.setElement(i, j, rottMatrix(i, j));
            CLOG(LINFO) << hm.getElement(i, j) << "  ";
		}
        hm.setElement(i, 3, avgTvec(i, 0));
        CLOG(LINFO) << hm.getElement(i, 3) <<" \n";
	}
	out_homogMatrix.write(hm);

	CLOG(LINFO)<<"Uśredniony kąt: " << avgFi;

	float standardDeviationFi = sqrt(pow(avgFi - fi, 2));

	CLOG(LINFO)<<"Odchylenie standardowe kąta: "<<standardDeviationFi;

}