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;
}
glm::mat4x3 proj4D()
{
// Infinite draw distance for now
// Also draws things behind, not ideal
glm::mat4x3 retMat(1);
return retMat;
}
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;
}
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);
}
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);
}
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;
}
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" );
}
}
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" );
}
}
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;
}
