void tests() { typedef RxSO3Group<Scalar> RxSO3Type; typedef typename RxSO3Group<Scalar>::Point Point; typedef typename RxSO3Group<Scalar>::Tangent Tangent; vector<RxSO3Type> rxso3_vec; rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0, 1.))); rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, -1.0, 1.1))); rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0., 1.1))); rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.))); rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.00001))); rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0))); rxso3_vec.push_back(RxSO3Type::exp(Tangent(M_PI, 0, 0, 0.9))); rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0,0)) *RxSO3Type::exp(Tangent(M_PI, 0, 0,0.0)) *RxSO3Type::exp(Tangent(-0.2, -0.5, -0.0,0))); rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.3, 0.5, 0.1,0)) *RxSO3Type::exp(Tangent(M_PI, 0, 0,0)) *RxSO3Type::exp(Tangent(-0.3, -0.5, -0.1,0))); vector<Tangent> tangent_vec; Tangent tmp; tmp << 0,0,0,0; tangent_vec.push_back(tmp); tmp << 1,0,0,0; tangent_vec.push_back(tmp); tmp << 1,0,0,0.1; tangent_vec.push_back(tmp); tmp << 0,1,0,0.1; tangent_vec.push_back(tmp); tmp << 0,0,1,-0.1; tangent_vec.push_back(tmp); tmp << -1,1,0,-0.1; tangent_vec.push_back(tmp); tmp << 20,-1,0,2; tangent_vec.push_back(tmp); vector<Point> point_vec; point_vec.push_back(Point(1,2,4)); Tests<RxSO3Type> tests; tests.setGroupElements(rxso3_vec); tests.setTangentVectors(tangent_vec); tests.setPoints(point_vec); tests.runAllTests(); }
void tests() { typedef SO2Group<Scalar> SO2Type; typedef SE2Group<Scalar> SE2Type; typedef typename SE2Group<Scalar>::Point Point; typedef typename SE2Group<Scalar>::Tangent Tangent; vector<SE2Type, Eigen::aligned_allocator<SE2Type> > se2_vec; se2_vec.push_back(SE2Type(SO2Type(0.0),Point(0,0))); se2_vec.push_back(SE2Type(SO2Type(0.2),Point(10,0))); se2_vec.push_back(SE2Type(SO2Type(0.),Point(0,100))); se2_vec.push_back(SE2Type(SO2Type(-1.),Point(20,-1))); se2_vec.push_back(SE2Type(SO2Type(0.00001), Point(-0.00000001,0.0000000001))); se2_vec.push_back(SE2Type(SO2Type(0.2),Point(0,0)) *SE2Type(SO2Type(M_PI),Point(0,0)) *SE2Type(SO2Type(-0.2),Point(0,0))); se2_vec.push_back(SE2Type(SO2Type(0.3),Point(2,0)) *SE2Type(SO2Type(M_PI),Point(0,0)) *SE2Type(SO2Type(-0.3),Point(0,6))); vector<Tangent, Eigen::aligned_allocator<Tangent> > tangent_vec; Tangent tmp; tmp << 0,0,0; tangent_vec.push_back(tmp); tmp << 1,0,0; tangent_vec.push_back(tmp); tmp << 0,1,1; tangent_vec.push_back(tmp); tmp << -1,1,0; tangent_vec.push_back(tmp); tmp << 20,-1,-1; tangent_vec.push_back(tmp); tmp << 30,5,20; tangent_vec.push_back(tmp); vector<Point, Eigen::aligned_allocator<Point> > point_vec; point_vec.push_back(Point(1,2)); Tests<SE2Type> tests; tests.setGroupElements(se2_vec); tests.setTangentVectors(tangent_vec); tests.setPoints(point_vec); tests.runAllTests(); }