int main(int argc, char *argv[]) {
Halide::Func data_mat;
Halide::Var row, col, ki, iter;
//Halide::Expr e = row + col;
// initialize data matrix
data_mat(row, col) = 1.0f;
// initialize model matrix L, R
Halide::Func L, R;
L(row, ki) = 1.0f;
R(ki, col) = 1.0f;
Halide::Func approx_mat;
approx_mat(row, col) = 0.0f;
Halide::RDom k(0, kRank);
approx_mat(row, col) = approx_mat(row, col) + L(row, k) * R(k, col);
Halide::Func diff_mat;
diff_mat(row, col) = approx_mat(row, col) - data_mat(row, col);
Halide::Func Ln, Rn;
Ln(row, ki) = 1.0f;
Rn(ki, col) = 1.0f;
Halide::RDom c(0, kWidth);
Ln(row, ki) = L(row, ki) - step_size * diff_mat(row, c) * R(ki, c);
Halide::RDom r(0, kHeight);
Rn(ki, col) = R(ki, col) - step_size * diff_mat(r, col) * L(r, ki);
Halide::Image<float> Ln_output = Ln.realize(kHeight, kRank);
Halide::Image<float> Rn_output = Rn.realize(kRank, kWidth);
for (int i = 0; i < Ln_output.height(); i++) {
std::cout << i << " - ";
for (int j = 0; j < Ln_output.width(); j++) {
std::cout << j <<":" << Ln_output(i, j) << " ";
}
std::cout << std::endl;
}
for (int i = 0; i < Rn_output.height(); i++) {
std::cout << i << " - ";
for (int j = 0; j < Rn_output.width(); j++) {
std::cout << j <<":" << Rn_output(i, j) << " ";
}
std::cout << std::endl;
}
std::cout << "Success!" << std::endl;
return 0;
}
void RpalParser::R()
{
pushProc("R()");
Rn();
while (_nt == "true" || _nt == "false" || _nt == "nil" || _nt == "dummy" || _nt == "(" ||
RpalScanner::IsInt(_nt) || RpalScanner::IsString(_nt) || RpalScanner::IsID(_nt))
{
Rn();
build("gamma", 2);
}
popProc("R()");
}
BufferOffset Assembler::DataProcShiftedRegister(const Register& rd, const Register& rn,
const Operand& operand, FlagsUpdate S, Instr op)
{
VIXL_ASSERT(operand.IsShiftedRegister());
VIXL_ASSERT(rn.Is64Bits() || (rn.Is32Bits() && is_uint5(operand.shift_amount())));
return Emit(SF(rd) | op | Flags(S) |
ShiftDP(operand.shift()) | ImmDPShift(operand.shift_amount()) |
Rm(operand.reg()) | Rn(rn) | Rd(rd));
}
int mvInt::interpret(const e3ga &amv, int creationFlags /* = 0*/) {
e3ga mv(amv);
const GAIM_FLOAT epsilon = 10e-8, epsilon2 = epsilon * epsilon;
const GAIM_FLOAT *c;
e3ga tmp, rt, pe1, pe2, pe3;
GAIM_FLOAT scale;
if (creationFlags == 0) creationFlags = m_creationFlags;
m_creationFlags = creationFlags;
m_type = MVI_E3GA;
int grade, type = mv.mvType(&grade), allGrades;
if (m_creationFlags & OC_VERSOR) type = GA_VERSOR; // force versor interpretation
if (m_creationFlags & OC_BLADE) type = GA_BLADE; // force blade interpretation
allGrades = 0X1FF; // OC_GRADE0 | ... | OC_GRADE8
if (m_creationFlags & allGrades) { // force grade X interpretation
grade = m_creationFlags & allGrades;
mv = mv(grade);
}
if (type == GA_BLADE) {
switch(grade) {
case GRADE0: // scalar
m_type |= MVI_SCALAR;
/*
scalar 0: magnitude
*/
m_scalar[0] = mv.scalar();
break;
case GRADE1:
m_type |= MVI_VECTOR;
/*
scalar 0: magnitude
vector 0: direction
*/
m_scalar[0] = sqrt(mv.norm_a());
c = mv[GRADE1];
if (fabs(m_scalar[0]) > 0.0) {
m_vector[0][0] = c[E3GA_E1] / m_scalar[0];
m_vector[0][1] = c[E3GA_E2] / m_scalar[0];
m_vector[0][2] = c[E3GA_E3] / m_scalar[0];
}
else {
m_vector[0][0] = m_vector[0][1] = m_vector[0][2] = 0.0;
}
m_point[0][0] = m_vector[0][0] * m_scalar[0];
m_point[0][1] = m_vector[0][1] * m_scalar[0];
m_point[0][2] = m_vector[0][2] * m_scalar[0];
break;
case GRADE2:
m_type |= MVI_BIVECTOR;
/*
scalar 0: magnitude
vector 0: normal
vector 1: orthogonal to vector 0 and 2
vector 2: orthogonal to vector 0 and 1
*/
m_scalar[0] = sqrt(mv.norm_a());
// set vector 0 (normal)
tmp = mv.dual() / m_scalar[0];
c = tmp[GRADE1];
m_vector[0][0] = c[E3GA_E1];
m_vector[0][1] = c[E3GA_E2];
m_vector[0][2] = c[E3GA_E3];
// set vector 1 and 2 (orthogonal to normal)
if ((g_state->m_e3gaFactor[0].norm_a() > 0.0) && // user factorization
(g_state->m_e3gaFactor[1].norm_a() > 0.0)) {
pe1 = ((g_state->m_e3gaFactor[0] << mv) << mv.inverse());
pe2 = ((g_state->m_e3gaFactor[1] << mv) << mv.inverse());
scale = (mv / (pe1 ^ pe2)).scalar();
//mv.print("target: ");
//(pe1 ^ pe2).print("1: ");
pe1 *= ((scale < 0.0) ? -1.0 : 1.0) * sqrt(fabs(scale));
pe2 *= sqrt(fabs(scale));
//(pe1 ^ pe2).print("2: ");
}
else { // auto factorization
e3gaRve3(rt, tmp);
pe1 = (rt * e3ga::e1 * rt.inverse())(GRADE1);
pe2 = (rt * e3ga::e2 * rt.inverse())(GRADE1);
}
c = pe1[GRADE1];
m_vector[1][0] = c[E3GA_E1]; m_vector[1][1] = c[E3GA_E2]; m_vector[1][2] = c[E3GA_E3];
c = pe2[GRADE1];
m_vector[2][0] = c[E3GA_E1]; m_vector[2][1] = c[E3GA_E2]; m_vector[2][2] = c[E3GA_E3];
break;
case GRADE3: // pseudoscalar / trivector
m_type |= MVI_TRIVECTOR;
/*
scalar 0: magnitude
*/
m_scalar[0] = mv[GRADE3][E3GA_I];
// factor into vector 0, 1, 2
if ( (scale = (g_state->m_e3gaFactor[0] ^ g_state->m_e3gaFactor[1] ^ g_state->m_e3gaFactor[2]).norm_a()) > 0.0) {
// factor using the presupplied factors
pe1 = g_state->m_e3gaFactor[0];
pe2 = g_state->m_e3gaFactor[1];
pe3 = g_state->m_e3gaFactor[2];
scale = pow(sqrt(scale), 1.0 / 3.0);
}
else {
// factor using the basis vectors
pe1 = e3ga::e1;
pe2 = e3ga::e2;
pe3 = e3ga::e3;
scale = 1.0;
}
//printf("Dividing by scale %f\n", scale);
// scale the factors
pe1 *= 1.0 / scale;
pe2 *= 1.0 / scale;
pe3 *= 1.0 / scale;
// set the factors
c = pe1[GRADE1];
m_vector[0][0] = c[E3GA_E1]; m_vector[0][1] = c[E3GA_E2]; m_vector[0][2] = c[E3GA_E3];
c = pe2[GRADE1];
m_vector[1][0] = c[E3GA_E1]; m_vector[1][1] = c[E3GA_E2]; m_vector[1][2] = c[E3GA_E3];
c = pe3[GRADE1];
m_vector[2][0] = c[E3GA_E1]; m_vector[2][1] = c[E3GA_E2]; m_vector[2][2] = c[E3GA_E3];
break;
}
}
else if (type == GA_VERSOR) {
GAIM_FLOAT odd = mv(GRADE1).norm_a() + mv(GRADE3).norm_a();
GAIM_FLOAT even = mv(GRADE0).norm_a() + mv(GRADE2).norm_a();
if (odd < even) { // even 3D versor = rotor
m_type |= MVI_ROTOR;
/*
scalar 0: magnitude
scalar 1: angle
vector 0: normal
vector 1: orthogonal to vector 0 and 2
vector 2: orthogonal to vector 0 and 1
point 0: position (todo for conformal?)
*/
even = sqrt(even);
m_scalar[0] = even;
// interpret the bivector part to retrieve plane
e3ga plane(m_scalar[0] * mv(GRADE2).normal());
mvInt bivectorInt(plane, OC_BLADE | OC_GRADE2);
memcpy(m_vector[0], bivectorInt.m_vector[0], sizeof(GAIM_FLOAT) * 3);
memcpy(m_vector[1], bivectorInt.m_vector[1], sizeof(GAIM_FLOAT) * 3);
memcpy(m_vector[2], bivectorInt.m_vector[2], sizeof(GAIM_FLOAT) * 3);
// retrieve the angle
e3ga Rn(mv / even);
GAIM_FLOAT ca = Rn.scalar();
GAIM_FLOAT sa = sqrt(Rn(GRADE2).norm_a());
if ((sa < epsilon) || (sin(acos(ca)) < epsilon)) m_scalar[1] = 0; // angle = 0
else m_scalar[1] = 2.0 * atan2(sa, ca);
//cprintf("Angle = %f\n", m_scalar[1]);
}
else { // odd 3D versor
m_type |= MVI_ODD_3D_VERSOR;
}
}
else {
m_type |= MVI_UNKNOWN;
m_valid = 0;
return 0;
}
m_valid = 1;
return 0;
}
int main(int argc, char **argv) {
if(argc<2) {
printf("specify output path");
return 1;
}
fp = fopen(argv[1],"w");
write("<svd>");
pcl::PointCloud<pcl::PointXYZ> pc1, pc2;
std::cout<<"\n";
for(int trans=0; trans<3*3*8*64; trans++) {
if( !(trans/(3*3*8))&1 && (trans/(3*3*8))&2 )
continue;
if( !(trans/(3*3*8))&4 && (trans/(3*3*8))&8 )
continue;
if( !(trans/(3*3*8))&16 && (trans/(3*3*8))&32 )
continue;
std::cout<<(trans*100/(10*10*8*64))<<"%\r";
Eigen::Quaternionf R;
Eigen::Vector3f t;
Eigen::Matrix3f mR_cmp;
generateTF(trans, R,t,mR_cmp);
for(int data=0; data<10; data++) {
pc1=generateData(data);
pc2=pc1;
transform(pc2,R,t);
for(int noise=0; noise<=10; noise++)
{
addNoise(pc2, noise);
//get transformation
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> trans_est;
Eigen::Matrix4f m=Eigen::Matrix4f::Identity();
trans_est.estimateRigidTransformation (pc1, pc2, m);
Eigen::Matrix3f m2;
for(int i=0; i<3; i++)
for(int j=0; j<3; j++)
m2(i,j)=m(i,j);
//compare to desired tf
Eigen::Quaternionf Rn(m2);
Eigen::Vector3f tn = m.col(3).head<3>();
float error_r = Rn.angularDistance(R);
float error_d = (tn-t).norm();
/*if(error_r>0.01) {
std::cout<<mR_cmp<<"\n";
Rn=mR_cmp;
std::cout<<Rn.toRotationMatrix()<<"\n";
Rn.normalize();
std::cout<<Rn.toRotationMatrix()<<"\n";
std::cout<<m2<<"\n";
std::cout<<"e: "<<error_r<<"\n";}*/
//write
write("<set>");
write("<trans>");
write((trans));
write("</trans>");
write("<data>");
write((data));
write("</data>");
write("<noise>");
write((noise));
write("</noise>");
write("<error_r>");
write((error_r));
write("</error_r>");
write("<error_d>");
write((error_d));
write("</error_d>");
write("</set>");
}
}
}
write("</svd>");
fclose(fp);
return 0;
}
BufferOffset Assembler::LogicalImmediate(const Register& rd, const Register& rn,
unsigned n, unsigned imm_s, unsigned imm_r, LogicalOp op)
{
unsigned reg_size = rd.size();
Instr dest_reg = (op == ANDS) ? Rd(rd) : RdSP(rd);
return Emit(SF(rd) | LogicalImmediateFixed | op | BitN(n, reg_size) |
ImmSetBits(imm_s, reg_size) | ImmRotate(imm_r, reg_size) | dest_reg | Rn(rn));
}