C++ (Cpp) bot_trans_invert Examples

Example #1

File: atrans.cpp Project: Patrick6289/navguide

/**
 * Returns: 1 on success, 0 on failure
 */
int 
atrans_get_nth_trans(ATrans *atrans, const char *from_frame,
        const char *to_frame, int nth_from_latest, 
        BotTrans *btrans, int64_t *timestamp)
{
    BotCTransLink *link = bot_ctrans_get_link(atrans->ctrans, from_frame, 
            to_frame);
    if(!link)
        return 0;
    int status = bot_ctrans_link_get_nth_trans(link, nth_from_latest, btrans, 
            timestamp);
    if(status && btrans && 
       0 != strcmp(to_frame, bot_ctrans_link_get_to_frame(link))) {
        bot_trans_invert(btrans);
    }
    return status;
}

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: ctrans.c Project: andybarry/pronto-distro

int
bot_ctrans_path_to_trans_latest(const BotCTransPath * path, BotTrans *result)
{
    bot_trans_set_identity(result);
    BotTrans temp_trans;
    for(int lind=0; lind<path->nlinks; lind++) {
        BotCTransLink *link = path->links[lind];
        int have_trans = _link_get_trans_latest(link, &temp_trans);
        if(!have_trans) {
            return 0;
        }
        if(path->invert[lind]) {
            bot_trans_invert(&temp_trans);
        }
        bot_trans_apply_trans(result, &temp_trans);
    }
    return 1;
}

Example #4

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;
}