Example #1
0
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()));
    
}
Example #2
0
///////////////////////////////////
//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;
  }    
}
Example #3
0
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;
}
Example #5
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;
}
Example #6
0
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;
}
Example #7
0
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()));
}
Example #8
0
// 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;
}
Example #9
0
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;  
}
Example #10
0
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;
}
Example #11
0
 Eigen::Isometry3d eigenRotationTransform() const
 {
   Eigen::Isometry3d r = Eigen::Isometry3d::Identity();
   r.rotate(eigenRotation());
   return r;
 }