C++ (Cpp) bot_trans_copy Examples

Example #1

File: trans.c Project: PennPanda/libbot2

void bot_trans_apply_trans_to(const BotTrans * src1, const BotTrans * src2, BotTrans * dest){
  BotTrans tmp; //allow in place operation
  bot_trans_copy(&tmp,src2);
  bot_trans_apply_trans(&tmp,src1);
  bot_trans_copy(dest,&tmp);
  return;
}

Example #2

File: main.cpp Project: Gastd/oh-distro

int 
main(int argc, char ** argv){
  
   double trans_vec[] = {-2.7363, 0.5958, -1.1588};
   double rot_quat[] = {0.8977, 0.0011, -9.0933e-04, -0.4407};
  
  BotTrans init_vicon, init_est, current_est;
  setBotTrans(&init_vicon, 1.4696,  0.5235, 0.8753, 0.9749, -0.0108, 0.0076, 0.2221);
  setBotTrans(&init_est, 2.1455, 1.8038, 1.1523, 0.8977, -0.0011, 9.0933e-04, 0.4407);
  setBotTrans(&current_est, 2.1456, 1.8038, 1.1523, 0.8976, -0.0011, 9.2603e-04, 0.4407);
  
  Eigen::Isometry3d init_vicon_e = setIsometry3dFromBotTrans(&init_vicon);
  Eigen::Isometry3d init_est_e = setIsometry3dFromBotTrans(&init_est);
  Eigen::Isometry3d current_est_e = setIsometry3dFromBotTrans(&current_est);
  
  std::cout << print_Isometry3d(current_est_e) << " current_est_e\n";  
  std::cout << print_Isometry3d(init_est_e) << " init_est_e\n";  
  Eigen::Isometry3d init_est_e_inv = init_est_e.inverse();
  std::cout << print_Isometry3d(init_est_e_inv) << " init_est_e.inverse\n";    
  
  Eigen::Isometry3d delta_e =  init_est_e_inv * current_est_e;
  std::cout << print_Isometry3d(delta_e) << " delta_e\n";    
  
  
  
  BotTrans init_est_inv;
  BotTrans* init_est_ptr = &init_est;
  bot_trans_copy(&init_est_inv, init_est_ptr);
  
  
  bot_trans_invert ( &init_est_inv );
  
  std::cout << print_BotTrans(&current_est) << " current_est\n";    

  
  std::cout << print_BotTrans(&init_est_inv) << " init_est_inv\n";    
  
  BotTrans delta;
  BotTrans* delta_ptr = &current_est;
  bot_trans_apply_trans( delta_ptr  ,  &init_est_inv );
  std::cout << print_BotTrans(&current_est) << " delta\n";    
  
  
  return 0;
}

Example #3

File: trans.c Project: PennPanda/libbot2

void bot_trans_invert_and_compose(const BotTrans * curr, const BotTrans * prev, BotTrans * delta){
  bot_trans_copy(delta,prev);
  bot_trans_invert(delta);
  bot_trans_apply_trans_to(delta,curr,delta);
  return;
}