C++ Pose::inverse方法代码示例

void estimateError(const Pose& P_ref, const Pose& P_test, double *dRa, double *dta)
{
    Vector3d t_test = P_test.translation().normalized();
    Vector3d t_ref = P_ref.translation().normalized();

    *dRa = (P_test * P_ref.inverse()).getAngle(); // delta rotation angle
    *dta = fabs(t_test.dot(t_ref)); // cosine of the angle between translation directions
}

void EstimateScales(double max_reproj_error, const Pose& Pa1,
    const Pose& Pa2,
    const Pose& Pb1,
    const std::vector<Vector2d>& yb1s,
    const std::vector<Vector2d>& yb2s,
    std::vector<double>& scale_nums,
    std::vector<double>& scale_dens,
    std::vector<double>& iratios)
{
    Pose Pba = Pb1 * Pa1.inverse(); // Known relative transform from camera a to b
    Pose Pa21 = Pa2 * Pa1.inverse(); // Relative transform from Pa1 to Pa2

    auto Rba = Pba.rotation(); // Must not be identity
    auto tba = Pba.translation();

    auto Pab = Pba.inverse();
    auto Rab = Pab.rotation();
    auto tab = Pab.translation();

    auto Ra21 = Pa21.rotation();
    auto ta21 = Pa21.translation();

    auto Rb21 = Rba * Ra21 * Rab; // Relative rotation from Pb1 to Pb2

    auto A = crossMatrix(tba + Rba * Ra21 * tab) * Rb21;
    auto B = crossMatrix(Rba * ta21) * Rb21;

#if 0
    int id = 2;
    printf("[%d] Tba = %s\n", id, toString(Tba).c_str());
    printf("[%d] Ta21 = %s\n", id, toString(Ta21).c_str());
    printf("[%d] Tab = %s\n", id, toString(Tab).c_str());
    printf("[%d] Rab = %s\n", id, toString(Ra21).c_str());
    printf("[%d] Ra21 = %s\n", id, toString(Ra21).c_str());
    printf("[%d] Rb21 = %s\n", id, toString(Rb21).c_str());
    printf("[%d] A = %s\n", id, toString(A).c_str());
    printf("[%d] B = %s\n", id, toString(B).c_str());
#endif
    uint n = (uint)yb1s.size();

    scale_nums.clear();
    scale_dens.clear();
    iratios.clear();

    for (uint i = 0; i < n; i++) {

        // Get a scale estimate from one correspondence pair in camera b

        auto y1 = yb1s[i].homogeneous();
        auto y2 = yb2s[i].homogeneous();

        double s_num = y2.dot(A * y1);
        double s_den = (-y2).dot(B * y1);
        double s = s_num / s_den;

        // Count the inliers 

        Pose Pb2 = Pba * Pose(Pa2.rotation(), s * Pa2.translation());
        Pose Pb21 = Pb2 * Pb1.inverse(); // Relative pose

        vector<bool> inliers(n, true);
        uint icnt = getReprojectionInliers(max_reproj_error, yb1s, yb2s, Pb21, inliers);

        iratios.push_back(icnt / (float)n);
        scale_nums.push_back(s_num);
        scale_dens.push_back(s_den);
    }
}

bool OPTCalibrationNode::save()
{
  // Save tfs between sensors and world coordinate system (last checherboard) to file
  std::string file_name = ros::package::getPath("opt_calibration") + "/conf/camera_poses.yaml";
  std::ofstream file;
  file.open(file_name.c_str());

  if (file.is_open())
  {
    ros::Time time = ros::Time::now();
    file << "# Auto generated file." << std::endl;
    file << "calibration_id: " << time.sec << std::endl << std::endl;

    Pose new_world_pose = Pose::Identity();

    if (world_computation_ == LAST_CHECKERBOARD)
    {
      new_world_pose = calibration_->getLastCheckerboardPose().inverse();
      for (size_t i = 0; i < sensor_vec_.size(); ++i)
      {
        const Sensor::Ptr & sensor = sensor_vec_[i];
        Pose pose = new_world_pose * sensor->pose();
        if(pose.translation().z() < 0)
        {
          ROS_INFO_STREAM("[" << sensor->frameId() << "] z < 0. Flipping /world orientation.");
          AngleAxis rotation(M_PI, Vector3(1.0, 1.0, 0.0).normalized());
          new_world_pose = rotation * calibration_->getLastCheckerboardPose().inverse();
          break;
        }
      }
    }
    else if (world_computation_ == UPDATE)
    {
      new_world_pose = fixed_sensor_pose_ * fixed_sensor_->pose().inverse();
    }

    // Write TF transforms between cameras and world frame
    file << "# Poses w.r.t. the \"world\" reference frame" << std::endl;
    file << "poses:" << std::endl;
    for (size_t i = 0; i < sensor_vec_.size(); ++i)
    {
      const Sensor::Ptr & sensor = sensor_vec_[i];

      Pose pose = new_world_pose * sensor->pose();

      file << "  " << sensor->frameId().substr(1) << ":" << std::endl;

      file << "    translation:" << std::endl
           << "      x: " << pose.translation().x() << std::endl
           << "      y: " << pose.translation().y() << std::endl
           << "      z: " << pose.translation().z() << std::endl;

      Quaternion rotation(pose.rotation());
      file << "    rotation:" << std::endl
           << "      x: " << rotation.x() << std::endl
           << "      y: " << rotation.y() << std::endl
           << "      z: " << rotation.z() << std::endl
           << "      w: " << rotation.w() << std::endl;

    }

    file << std::endl << "# Inverse poses" << std::endl;
    file << "inverse_poses:" << std::endl;
    for (size_t i = 0; i < sensor_vec_.size(); ++i)
    {
      const Sensor::Ptr & sensor = sensor_vec_[i];

      Pose pose = new_world_pose * sensor->pose();
      pose = pose.inverse();

      file << "  " << sensor->frameId().substr(1) << ":" << std::endl;

      file << "    translation:" << std::endl
           << "      x: " << pose.translation().x() << std::endl
           << "      y: " << pose.translation().y() << std::endl
           << "      z: " << pose.translation().z() << std::endl;

      Quaternion rotation(pose.rotation());
      file << "    rotation:" << std::endl
           << "      x: " << rotation.x() << std::endl
           << "      y: " << rotation.y() << std::endl
           << "      z: " << rotation.z() << std::endl
           << "      w: " << rotation.w() << std::endl;

    }

  }
  file.close();

  ROS_INFO_STREAM(file_name << " created!");

  return true;

}