void motion3s(Eigen::Matrix3f &rot, Eigen::Vector3f &tr)
{
Eigen::Vector3f n=Eigen::Vector3f::Identity();
Eigen::AngleAxisf aa(0.05f,n);
rot = aa.toRotationMatrix();
tr.fill(0);
}
void motion3(Eigen::Matrix3f &rot, Eigen::Vector3f &tr)
{
Eigen::Vector3f n;
n(0) = gen_random_float(-1,1);
n(1) = gen_random_float(-1,1);
n(2) = gen_random_float(-1,1);
n.normalize();
Eigen::AngleAxisf aa(gen_random_float(-0.02f,0.05f),n);
rot = aa.toRotationMatrix();
tr.fill(0);
}
//TEST(Slam,sim_run)
void t4()
{
//*s means simple motion (for debug purpose)
motion_fct motion[]={motion1,motion2s,motion3s,motion4s,motion2,motion3,motion4,0};
int mp=0;
while(motion[mp])
{
Eigen::Matrix3f path_rot=Eigen::Matrix3f::Identity(),rot=Eigen::Matrix3f::Identity(),tmp_rot,tmp_rot2;
Eigen::Vector3f path_tr=Eigen::Vector3f::Zero(),tr=Eigen::Vector3f::Zero(),tmp_tr,tmp_tr2;
//input data
std::vector<cob_3d_mapping_msgs::CurvedPolygon> planes = generateRandomPlanes(5, true);
//setup slam
typedef DOF6::DOF6_Source<DOF6::TFLinkvf,DOF6::DOF6_Uncertainty<Dummy::RobotParameters,float> > DOF6;
typedef Slam::Node<Slam_CurvedPolygon::OBJCTXT<DOF6> > Node;
Slam::Context<Slam_CurvedPolygon::KEY<DOF6>, Node> ctxt(1,1);
//run
for(int i=0; i<15; i++)
{
std::cout<<"real rot\n"<<rot<<"\n";
std::cout<<"real tr\n"<<tr<<"\n";
path_rot = rot*path_rot;
path_tr += tr;
ctxt.startFrame(i);
for(size_t j=0; j<planes.size(); j++)
{
ctxt += planes[j];
}
ctxt.finishFrame();
//get motion
motion[mp](rot,tr);
//transform
for(size_t j=0; j<planes.size(); j++)
{
Eigen::Vector3f v,t;
v(0)=planes[j].features[0].x;
v(1)=planes[j].features[0].y;
v(2)=planes[j].features[0].z;
v=rot*v;
t=v*(tr.dot(v))/(v.squaredNorm());
ROS_ASSERT(t.squaredNorm()<=tr.squaredNorm());
v+=t;
planes[j].features[0].x=v(0);
planes[j].features[0].y=v(1);
planes[j].features[0].z=v(2);
Eigen::Vector3f n=v;
planes[j].parameter[0] = n.norm();
n/=n(2);
planes[j].parameter[1] = -n(0);
planes[j].parameter[3] = -n(1);
Eigen::Vector3f z;
z.fill(0);z(2)=1.f;
n.normalize();
planes[j].parameter[0] /= (n.dot(z));
}
//check path
tmp_rot = Eigen::Matrix3f::Identity();
tmp_tr = Eigen::Vector3f::Zero();
tmp_rot2 = Eigen::Matrix3f::Identity();
tmp_tr2 = Eigen::Vector3f::Zero();
const Slam::SWAY<Node> *n = &ctxt.getPath().getLocal();
while(n)
{
tmp_tr += n->link_.getTranslation();
tmp_rot = ((Eigen::Matrix3f)n->link_.getRotation())*tmp_rot;
tmp_tr2 += tmp_rot2.inverse()*n->link_.getSource1()->getTranslation();
tmp_rot2 = ((Eigen::Matrix3f)n->link_.getSource1()->getRotation())*tmp_rot2;
std::cout<<"con\n";
n = n->node_->getConnections().size()?&n->node_->getConnections()[0]:NULL;
}
std::cout<<"ROT1\n"<<tmp_rot<<"\n";
std::cout<<"ROT2\n"<<tmp_rot2<<"\n";
std::cout<<"ROT3\n"<<path_rot<<"\n";
std::cout<<"TR1\n"<<tmp_tr<<"\n";
std::cout<<"TR2\n"<<tmp_tr2<<"\n";
std::cout<<"TR3\n"<<path_tr<<"\n";
MATRIX_DISTANCE(tmp_rot, path_rot);
MATRIX_DISTANCE(tmp_tr, path_tr);
}
mp++;
}
}
void motion1(Eigen::Matrix3f &rot, Eigen::Vector3f &tr)
{
rot = Eigen::Matrix3f::Identity();
tr.fill(0);
}
int main(int argc, char **argv){
#if 0
DOF6::TFLinkvf rot1,rot2;
Eigen::Matrix4f tf1 = build_random_tflink(rot1,3);
Eigen::Matrix4f tf2 = build_random_tflink(rot2,3);
DOF6::DOF6_Source<DOF6::TFLinkvf,DOF6::TFLinkvf,float> abc(rot1.makeShared(), rot2.makeShared());
abc.getRotation();
abc.getTranslation();
std::cout<<"tf1\n"<<tf1<<"\n";
std::cout<<"tf2\n"<<tf2<<"\n";
std::cout<<"tf1*2\n"<<tf1*tf2<<"\n";
std::cout<<"tf2*1\n"<<tf2*tf1<<"\n";
std::cout<<"tf1\n"<<rot1.getTransformation()<<"\n";
std::cout<<"tf2\n"<<rot2.getTransformation()<<"\n";
std::cout<<"tf1*2\n"<<(rot1+rot2).getTransformation()<<"\n";
rot1.check();
rot2.check();
return 0;
pcl::RotationFromCorrespondences rfc;
Eigen::Vector3f n, nn, v,n2,n3,z,y,tv;
float a = 0.1f;
z.fill(0);y.fill(0);
z(2)=1;y(1)=1;
nn.fill(0);
nn(0) = 1;
Eigen::AngleAxisf aa(a,nn);
nn.fill(100);
n.fill(0);
n(0) = 1;
n2.fill(0);
n2=n;
n2(1) = 0.2;
n3.fill(0);
n3=n;
n3(2) = 0.2;
n2.normalize();
n3.normalize();
tv.fill(1);
tv.normalize();
#if 0
#if 0
rfc.add(n,aa.toRotationMatrix()*n+nn,
1*n.cross(y),1*n.cross(z),
1*(aa.toRotationMatrix()*n).cross(y),1*(aa.toRotationMatrix()*n).cross(z),
1,1/sqrtf(3));
#else
rfc.add(n,aa.toRotationMatrix()*n,
0*n.cross(y),0*n.cross(z),
0*(aa.toRotationMatrix()*n).cross(y),0*(aa.toRotationMatrix()*n).cross(z),
1,0);
#endif
#if 1
rfc.add(n2,aa.toRotationMatrix()*n2+nn,
tv,tv,
tv,tv,
1,1);
#else
rfc.add(n2,aa.toRotationMatrix()*n2+nn,
1*n2.cross(y),1*n2.cross(z),
1*(aa.toRotationMatrix()*n2).cross(y),1*(aa.toRotationMatrix()*n2).cross(z),
1,1/sqrtf(3));
#endif
#else
float f=1;
Eigen::Vector3f cyl;
cyl.fill(1);
cyl(0)=1;
Eigen::Matrix3f cylM;
cylM.fill(0);
cylM.diagonal() = cyl;
rfc.add(n,aa.toRotationMatrix()*n,
f*n.cross(y),f*n.cross(z),
f*(aa.toRotationMatrix()*n).cross(y),f*(aa.toRotationMatrix()*n).cross(z),
1,0);
rfc.add(n2,aa.toRotationMatrix()*n2+nn,
1*n2.cross(y),1*n2.cross(z),
1*(aa.toRotationMatrix()*n2).cross(y),1*(aa.toRotationMatrix()*n2).cross(z),
1,1);
#endif
rfc.add(n3,aa.toRotationMatrix()*n3+nn,
//tv,tv,
//tv,tv,
n3.cross(y),n3.cross(z),
1*(aa.toRotationMatrix()*n3).cross(y),1*(aa.toRotationMatrix()*n3).cross(z),
1,1);
std::cout<<"comp matrix:\n"<<rfc.getTransformation()<<"\n\n";
std::cout<<"real matrix:\n"<<aa.toRotationMatrix()<<"\n\n";
return 0;
//rfc.covariance_.normalize();
rfc.covariance_ = (rfc.var_*rfc.covariance_.inverse().transpose()*rfc.covariance_);
std::cout<<"comp matrix: "<<rfc.getTransformation()<<"\n\n";
std::cout<<"real matrix: "<<aa.toRotationMatrix()*aa.toRotationMatrix()<<"\n\n";
return 0;
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
