arma::mat CartesianDMPLearner::computeFitY(arma::vec& time, arma::mat &y, arma::mat &dy, arma::mat &ddy, arma::vec& vec_g) {

        // position
        mat retMat(y.n_cols - 1, y.n_rows);

        for(int i = 0; i < y.n_rows; ++i) {

            for(int j = 0; j < 3; ++j) {

                double yVal = y(i, j);
                double dyVal = dy(i, j);
                double ddyVal = ddy(i, j);
                retMat(j, i) = /*1 / (vec_g(j) - y(0, j)) */ tau * tau * ddyVal - az * (bz * (vec_g(j) - yVal) - tau * dyVal);

            }

        }

        // orientation
        arma::mat omega(y.n_rows, 3);
        arma::mat domega;
        arma::mat eta;
        arma::mat deta;

        for (int j = 0; j < y.n_rows - 1; ++j) {

            vec logL= log(tf::Quaternion(y(j + 1, 3), y(j + 1,  4), y(j + 1, 5), y(j + 1, 6)) * tf::Quaternion(y(j, 3), y(j, 4), y(j, 5), y(j, 6)).inverse());

            for (int i = 0; i < 3; i++)
                omega(j, i) = 2 * logL(i) / (time(1)-time(0));

            if (j == y.n_rows - 2)
                for (int i = 0; i < 3; i++)
                    omega(y.n_rows - 1, i) = 2 * logL(i) / (time(1)-time(0));

        }

        for(int i = 0; i < 3 ; ++i) {

            vec trajectory = omega.col(i);
            vec domegaV = computeDiscreteDerivatives(time, trajectory);
            domega = join_rows(domega, domegaV);

        }


        eta = tau * omega;
        deta = tau * domega;

        for (int i = 0; i < y.n_rows; ++i) {

            vec logL = log(tf::Quaternion(vec_g(3), vec_g(4), vec_g(5), vec_g(6)) * tf::Quaternion(y(i, 3), y(i, 4), y(i, 5), y(i, 6)).inverse());
            for (int j = 3; j < retMat.n_rows; ++j)
                retMat(j, i) = tau * deta (i, j - 3) - az * (bz * 2 * logL(j - 3) - eta(i, j - 3));

        }

        return retMat;

    }
Exemple #2
0
glm::mat4x3 proj4D()
{
    // Infinite draw distance for now
    // Also draws things behind, not ideal
    glm::mat4x3 retMat(1);
    return retMat;
}
Exemple #3
0
Matrix Matrix::add(double c) const {
	mat retMat(numRow, vec(numCol, 0));
	for(int i=0; i<numRow; i++){
		for(int j=0; j<numCol; j++){
			retMat[i][j]=primitive[i][j]+c;
		}
	}
	return Matrix(retMat);
}
obvious::Matrix ThreadLocalize::maskMatrix(obvious::Matrix* Mat, bool* mask, unsigned int maskSize, unsigned int validPoints)
{
  assert(Mat->getRows() == maskSize);
  assert(Mat->getCols() == 2);
  obvious::Matrix retMat(validPoints, 2);
  unsigned int cnt = 0;

  for(unsigned int i= 0; i < maskSize; i++)
  {
    if(mask[i])
    {
      retMat(cnt, 0) = (*Mat)(i, 0);
      retMat(cnt, 1) = (*Mat)(i, 1);
      cnt++;
    }
  }
  return retMat;
}
Exemple #5
0
Matrix Matrix::subtract(double c) const {
	mat retMat(numRow, std::vector<double>(numCol, 0));
	for(int i=0; i<numRow; i++){
		for(int j=0; j<numCol; j++){
			retMat[i][j]=primitive[i][j]-c;
		}
	}
	return Matrix(retMat);
}
Exemple #6
0
Matrix Matrix::multiply(double c) const {
	//Matrix plac=Matrix(c*real, c*im);
	mat retMat(numRow, vec(numCol, 0));
	for(int i=0; i<numRow; i++){
		for(int j=0; j<numCol; j++){
			retMat[i][j]=primitive[i][j]*c;
		}
	}
	return Matrix(retMat);
}
Exemple #7
0
arma::mat JointDMPLearner::computeFitY(arma::vec& time, arma::mat &y, arma::mat &dy, arma::mat &ddy, arma::vec& vec_g) {

    mat retMat(y.n_cols, y.n_rows);
    for(int i = 0; i < y.n_rows; ++i) {

        for(int j = 0; j < y.n_cols; ++j) {

            double yVal = y(i, j);
            double dyVal = dy(i, j);
            double ddyVal = ddy(i, j);
            retMat(j, i) = tau * tau * ddyVal - az * (bz * (vec_g(j) - yVal) - tau * dyVal);

        }

    }

    return retMat;

}
// Point2f構造体vectorを斉次座標にしてMat型へ変換
Mat transPointVecToMatHom(vector<Point2f>& pt_vec)
{
	int size = pt_vec.size();
	Mat retMat(3, size, CV_64FC1);

	for(int i=0; i<size; i++){
		retMat.at<double>(0,i) = (double)(pt_vec[i].x);
		retMat.at<double>(1,i) = (double)(pt_vec[i].y);
		retMat.at<double>(2,i) = 1.0;
	}

	return retMat;
}
Exemple #9
0
Matrix Matrix::subtract(const Matrix &c) const {
	if(numRow==c.getNumRow()&&numCol==c.getNumCol()){
		mat retMat(numRow, vec(numCol, 0));
		for(int i=0; i<numRow; i++){
			for(int j=0; j<numCol; j++){
				retMat[i][j]=primitive[i][j]-c[i][j];
			}
		}
	}
	else {
		throw std::invalid_argument( "matrix does not match" );
	}
}
Exemple #10
0
Matrix Matrix::multiply(const vec &c) const {
	if(numCol==c.size()){
		mat retMat(numRow, vec(1, 0));
		for(int k=0; k<numRow; k++){
			for(int i=0; i<numCol; i++){
				retMat[k][0]+=primitive[k][i]*c[i];
			}

		}
		return Matrix(retMat);
	}
	else if(numRow==c.size()){
		mat retMat(1, vec(numCol, 0));
		for(int k=0; k<numRow; k++){
			for(int i=0; i<numCol; i++){
				retMat[0][i]+=primitive[k][i]*c[k];
			}
		}
		return Matrix(retMat);
	}
	else {
		throw std::invalid_argument( "matrix does not match" );
	}
}
Exemple #11
0
Matrix Matrix::multiply(const Matrix &c) const {
	if(numCol==c.getNumRow()){
		mat retMat(numRow, vec(c.getNumCol(), 0));
		for(int k=0; k<numRow; k++){
			for(int j=0; j<c.getNumCol(); j++){
				for(int i=0; i<numCol; i++){
					retMat[k][j]+=primitive[k][i]*c[i][j];
				}
			}
		}
		return Matrix(retMat);
	}
	else {
		throw std::invalid_argument( "matrix does not match" );
	}
}
Mat transPointVecToMat2D(vector<Point2f>& pt_vec, vector<unsigned char>& mask)
{
	CV_Assert(mask.empty() || mask.size() == pt_vec.size());

	vector<unsigned char>::iterator s_itr;
	vector<Point2f>::iterator pt_itr;

	int i;
	int size;
	if(mask.empty()){
		size = pt_vec.size();
		for(i=0; i< size; i++){
			mask.push_back(1);
		}
	}
	else{
		size = 0;
		s_itr = mask.begin();
		while(s_itr != mask.end()){
			if(*s_itr > 0)
				size++;
			s_itr++;
		}
	}

	Mat retMat(size, 1, CV_32FC2);

	s_itr = mask.begin();
	pt_itr = pt_vec.begin();
	int col = 0;
	while(pt_itr != pt_vec.end()){
		if(*s_itr > 0){
			retMat.at<Vec2f>(col,0)[0] = (*pt_itr).x;
			retMat.at<Vec2f>(col,0)[1] = (*pt_itr).y;
//			retMat.at<float>(col,0) = (float)((*pt_itr).x);
//			retMat.at<float>(col,0) = (double)((*pt_itr).y);
			col++;
		}
		pt_itr++;
		s_itr++;
	}

	return retMat;
}
void fileProcess(const std::string& inputFileName,
		 const std::string& outputFileName,
		 int rank){
  double startSec = Util::getSec();
  std::cout << "read matrix from " << inputFileName << " ... " << std::flush;
  Mat A;
  readMatrix(inputFileName.c_str(), A);
  std::cout << Util::getSec() - startSec << " sec." <<std:: endl;
  std::cout << "rows:\t" << A.rows() << std::endl
	    << "cols:\t" << A.cols() << std::endl
	    << "rank:\t" << rank  << std::endl;

  std::cout << "compute ... " << std::flush;
  startSec = Util::getSec();
  RetMat retMat(A, rank);
  std::cout << Util::getSec() - startSec << " sec." << std::endl;
  
  startSec = Util::getSec();
  writeMatrix(outputFileName, retMat);
  std::cout << Util::getSec() - startSec << " sec." << std::endl
	    << "finished." << std::endl;
}