void DH2HG(Eigen::Isometry3d &B, double t, double f, double r, double d)
{
// Convert DH parameters (standard convention) to Homogenuous transformation matrix.
B = Eigen::Matrix4d::Identity();
B.translate(Eigen::Vector3d(0.,0.,d));
B.rotate(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ()));
B.translate(Eigen::Vector3d(r,0,0));
B.rotate(Eigen::AngleAxisd(f, Eigen::Vector3d::UnitX()));
}
///////////////////////////////////
//time x y z qx qy qz qw - all floating points
void read_poses_csv(std::string poses_files, std::vector<Isometry3dTime>& poses){
int counter=0;
string line;
ifstream myfile (poses_files.c_str());
if (myfile.is_open()){
while ( myfile.good() ){
getline (myfile,line);
if (line.size() > 4){
double quat[4];
double pos[3];
int64_t dtime;
sscanf(line.c_str(),"%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
&dtime,
&pos[0],&pos[1],&pos[2],
&quat[1],&quat[2],&quat[3],&quat[0]); // NBNBN: note the order
Eigen::Quaterniond quat2(quat[0],quat[1],quat[2],quat[3]);
Eigen::Isometry3d pose;
pose.setIdentity();
pose.translation() << pos[0] ,pos[1],pos[2];
pose.rotate(quat2);
Isometry3dTime poseT(dtime, pose);
counter++;
poses.push_back(poseT);
}
}
myfile.close();
} else{
printf( "Unable to open poses file\n%s",poses_files.c_str());
return;
}
}
Eigen::Isometry3d matrixdToIsometry3d(Matrix<double,7,1> input){
Eigen::Isometry3d output;
output.setIdentity();
output.translation() = Eigen::Vector3d( input[0], input[1], input[2] );
Eigen::Quaterniond quat = Eigen::Quaterniond(input[3], input[4], input[5], input[6]);
output.rotate(quat);
return output;
}
int main(int argc, char** argv)
{
ROS_INFO("Tracker Started.");
ros::init(argc, argv, "simple_tracker");
ros::NodeHandle nh;
ros::ServiceClient poseClient = nh.serviceClient<HuboApplication::SetHuboArmPose>("/hubo/set_arm");
ros::Rate loop_rate(0.1);
while (ros::ok())
{
/*double x = randbetween(X_MIN, X_MAX);
double y = randbetween(Y_MIN, Y_MAX);
double z = randbetween(Z_MIN, Z_MAX);*/
Eigen::Isometry3d ePose;
tf::StampedTransform tPose;
HuboApplication::SetHuboArmPose srv;
ePose.matrix() << 1, 0, 0, .3,
0, 1, 0, .2,
0, 0, 1, 0,
0, 0, 0, 1;
ePose.rotate(Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d::UnitZ()));
ePose.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
/*
Collision_Checker cc;
cc.initCollisionChecker();
cc.checkSelfCollision(ePose);
tf::TransformEigenToTF(ePose, tPose);
tf::poseTFToMsg(tPose, srv.request.Target);
srv.request.ArmIndex = 1;
poseClient.call(srv);
*/
ros::spinOnce();
loop_rate.sleep();
}
ros::spin();
return 0;
}
Eigen::Isometry3d setIsometry3dFromBotTrans(BotTrans *bt){
Eigen::Isometry3d tf;
tf.setIdentity();
tf.translation() << bt->trans_vec[0], bt->trans_vec[1],bt->trans_vec[2];
Eigen::Quaterniond q = Eigen::Quaterniond(bt->rot_quat[0], bt->rot_quat[1], bt->rot_quat[2], bt->rot_quat[3]);
tf.rotate(q);
return tf;
}
Eigen::Isometry3d transformEstimation(const Mat& rgb1,const Mat& rgb2,const Mat& depth1,const Mat& depth2,const CAMERA_INTRINSIC_PARAMETERS& C)
{
vector<KeyPoint> keyPts1,keyPts2;
Mat descriptors1,descriptors2;
extractKeypointsAndDescripotrs(rgb1,rgb2,keyPts1,keyPts2,descriptors1,descriptors2);
vector<DMatch> matches;
descriptorsMatch(rgb1,rgb2,keyPts1,keyPts2,descriptors1,descriptors2,matches);
vector<Point2f> points;
vector<Point3f> objectPoints;
vector<Eigen::Vector2d> imagePoints1,imagePoints2;
getObjectPointsAndImagePoints(depth1,depth2,keyPts1,keyPts2,matches,points,objectPoints,imagePoints1,imagePoints2,C);
Mat translation,rotation;
double camera_matrix_data[3][3] = {
{C.fx, 0, C.cx},
{0, C.fy, C.cy},
{0, 0, 1} };
Mat cameraMatrix(3,3,CV_64F,camera_matrix_data);
solvePnPRansac(objectPoints,points,cameraMatrix,Mat(),rotation,translation,false, 100, 1.0, 20);
Mat rot;
Rodrigues(rotation,rot);
Eigen::Matrix3d r;
Eigen::Vector3d t;
cout<<rot<<endl;
cout<<translation<<endl;
r<< ((double*)rot.data)[0],((double*)rot.data)[1],((double*)rot.data)[2],
((double*)rot.data)[3],((double*)rot.data)[4],((double*)rot.data)[5],
((double*)rot.data)[6],((double*)rot.data)[7],((double*)rot.data)[8];
t<<((double*)translation.data)[0],((double*)translation.data)[1],((double*)translation.data)[2];
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(r);
T.pretranslate(t);
cout<<T.matrix()<<endl;
BundleAdjustmentOptimization(objectPoints,imagePoints1,imagePoints2);
return T;
}
void randomize_transform(Eigen::Isometry3d& tf,
double translation_limit=100,
double rotation_limit=100)
{
Eigen::Vector3d r = random_vec<3>(translation_limit);
Eigen::Vector3d theta = random_vec<3>(rotation_limit);
tf.setIdentity();
tf.translate(r);
if(theta.norm()>0)
tf.rotate(Eigen::AngleAxisd(theta.norm(), theta.normalized()));
}
// TODO: consider orientations other than identity
std::vector<reachability_t> TestReachability(double rotation)
{
const double XMIN = -0.1, XMAX = 0.7, YMIN = -0.8, YMAX = .2, ZMIN = -1, ZMAX =
0.3, STEPSIZE = 0.05;
HK::HuboKin hubo;
std::vector<reachability_t> results;
Eigen::VectorXd weight(7);
weight << 1, 1, 1, 1, 1, 1, 1; //change this for weights!
Vector6d q;
int count = 0, valid = 0;
for (double x = XMIN; x <= XMAX; x += STEPSIZE)
{
for (double y = YMIN; y <= YMAX; y += STEPSIZE)
{
for (double z = ZMIN; z <= ZMAX; z += STEPSIZE)
{
++count;
Eigen::Isometry3d target = Eigen::Isometry3d::Identity();
Eigen::Isometry3d result;
target.translation().x() = x;
target.translation().y() = y;
target.translation().z() = z;
target.rotate(Eigen::AngleAxisd(-(M_PI/6) * rotation, Eigen::Vector3d::UnitX()));
hubo.armIK(q, target, Vector6d::Zero(), RIGHT);
hubo.armFK(result, q, RIGHT);
double ret = compareT(target, result, weight);
//std::cout << ret << "\t";
if (ret > 0.15)
{
continue;
}
else
{
valid++;
reachability_t pointScore;
pointScore.x = x;
pointScore.y = y;
pointScore.z = z;
pointScore.error = ret;
results.push_back(pointScore);
}
}
}
}
return results;
}
std::vector<ImageFeature> RegApp::getFeaturesFromLCM(const reg::features_t* msg, Eigen::Isometry3d &pose){
std::vector<ImageFeature> features;
for (int i =0; i < msg->nfeatures; i++){
ImageFeature f;
f.track_id = msg->features[i].track_id;
f.uv[0] = msg->features[i].uv[0];
f.uv[1] = msg->features[i].uv[1];
f.base_uv[0] = msg->features[i].base_uv[0];
f.base_uv[1] = msg->features[i].base_uv[1];
f.uvd[0] = msg->features[i].uvd[0];
f.uvd[1] = msg->features[i].uvd[1];
f.uvd[2] = msg->features[i].uvd[2];
f.xyz[0] = msg->features[i].xyz[0];
f.xyz[1] = msg->features[i].xyz[1];
f.xyz[2] = msg->features[i].xyz[2];
f.xyzw[0] = msg->features[i].xyzw[0];
f.xyzw[1] = msg->features[i].xyzw[1];
f.xyzw[2] = msg->features[i].xyzw[2];
f.xyzw[3] = msg->features[i].xyzw[3];
// color left out for now
pose.setIdentity();
pose.translation() << msg->pos[0], msg->pos[1], msg->pos[2];
Eigen::Quaterniond m(msg->quat[0],msg->quat[1],msg->quat[2],msg->quat[3]);
pose.rotate(m);
/*
cout << line << " is line\n";
cout << "i: " << i <<"\n";
cout << "f.track_id: " << f.track_id <<"\n";
cout << "f.uv: " << f.uv[0] << " "<< f.uv[1] <<"\n";
cout << "f.base_uv: " << f.base_uv[0] << " "<< f.base_uv[1] <<"\n";
cout << "f.uvd: " << f.uvd[0] << " "<< f.uvd[1]<< " "<< f.uvd[2]<<"\n";
cout << "f.xyz: " << f.xyz[0] << " "<< f.xyz[1]<< " "<< f.xyz[2]<<"\n";
cout << "f.xyzw: " << f.xyzw[0] << " "<< f.xyzw[1]<< " "<< f.xyzw[2]<< " "<< f.xyzw[3]<<"\n";
cout << "f.color: " << (int)f.color[0] << " "<< (int)f.color[1] << " "<< (int)f.color[2] <<"\n";
*/
features.push_back(f);
}
//cout << "in: " << msg->nfeatures << " | extracted: "<< features.size() << "\n";
return features;
}
int
main (int argc, char** argv)
{
// 1. Parse arguments:
print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
if (argc < 3)
{
printHelp (argc, argv);
return (-1);
}
int mode=atoi(argv[1]);
// 2 Construct the simulation method:
int width = 640;
int height = 480;
simexample = SimExample::Ptr (new SimExample (argc, argv, height,width ));
// 3 Generate a series of simulation poses:
// -0 100 fixed poses
// -1 a 'halo' camera
// -2 slerp between two different poses
std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
if (mode==0){
// Create a pose:
Eigen::Isometry3d pose;
pose.setIdentity();
Matrix3d m;
//ypr:
m = AngleAxisd(-9.14989, Vector3d::UnitZ()) * AngleAxisd(0.20944, Vector3d::UnitY()) * AngleAxisd(0, Vector3d::UnitX());
pose *= m;
Vector3d v;
v << 1.31762, 0.382931, 1.89533;
pose.translation() = v;
for (int i=0;i< 100;i++){ // duplicate the pose 100 times
poses.push_back(pose);
}
}else if(mode==1){
Eigen::Vector3d focus_center(0,0,1.3);
double halo_r = 4;
double halo_dz = 2;
int n_poses=16;
generate_halo(poses,focus_center,halo_r,halo_dz,n_poses);
}else if (mode==2){
Eigen::Isometry3d pose1;
pose1.setIdentity();
pose1.translation() << 1,0.75,2;
Eigen::Matrix3d rot1;
rot1 = AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())
* AngleAxisd(M_PI/10, Eigen::Vector3d::UnitY())
* AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
pose1.rotate(rot1);
Eigen::Isometry3d pose2;
pose2.setIdentity();
pose2.translation() << 1,-1,3;
Eigen::Matrix3d rot2;
rot2 = AngleAxisd(3*M_PI/4, Eigen::Vector3d::UnitZ())
* AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY())
* AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
pose2.rotate(rot2);
int n_poses = 20;
for (double i=0; i<=1;i+= 1/((double) n_poses -1) ){
Eigen::Quaterniond rot3;
Eigen::Quaterniond r1(pose1.rotation());
Eigen::Quaterniond r2(pose2.rotation());
rot3 = r1.slerp(i,r2);
Eigen::Isometry3d pose;
pose.setIdentity();
Eigen::Vector3d trans3 = (1-i)*pose1.translation() + i*pose2.translation();
pose.translation() << trans3[0] ,trans3[1] ,trans3[2];
pose.rotate(rot3);
poses.push_back(pose);
}
}
// 4 Do the simulation and write the output:
double tic_main = getTime();
for (size_t i=0;i < poses.size();i++){
std::stringstream ss;
ss.precision(20);
ss << "simcloud_" << i;// << ".pcd";
double tic = getTime();
simexample->doSim(poses[i]);
write_sim_output(ss.str());
cout << (getTime() -tic) << " sec\n";
}
cout << poses.size() << " poses simulated in " << (getTime() -tic_main) << "seconds\n";
cout << (poses.size()/ (getTime() -tic_main) ) << "Hz on average\n";
return 0;
}
Eigen::Isometry3d eigenRotationTransform() const
{
Eigen::Isometry3d r = Eigen::Isometry3d::Identity();
r.rotate(eigenRotation());
return r;
}