Question

我目前正在研究专用于机器人校准的ros软件包，使用Ceres进行模型优化。基本上，此软件包的目标是闭合包括任何机器人和6d传感器的几何回路，并在每个机器人链接之间插入优化参数。对于N轴机器人，参数分为N * 2个参数块，将每个链接分成两个块，一个用于平移（x，y，z），一个用于定向（qx，qy，qz，qw）。局部参数化EigenQuaternionParameterization应用于每个方向块。

我成功地通过数值微分使用了它，现在尝试使用DynamicAutoDiffCostFunction，但是似乎我无法使Jacobian评估/计算正常工作。

这是我经过一次独特的迭代后得到的终止消息：

Termination:                      CONVERGENCE (Gradient tolerance reached. Gradient max norm: 0.000000e+00 <= 1.000000e-10)

这是我的operator（）：

template <typename T>
  bool operator()(T const *const *free_params, T *residuals) const {

    // Update calibration offsets based on free params
    offsets_->update(free_params[0]);

    // Arm FK
    KDL::Frame arm_pose_kdl;
    Eigen::Affine3d arm_pose;

    arm_pose_kdl = arm_model_->getChainFK(*offsets_, data_.joint_values,
                                          data_.joint_names);
    kdlToEigen3dPose(arm_pose_kdl, arm_pose);

    // flangeHcalpattern offset
    KDL::Frame flangeHcalpattern_offset_kdl;
    Eigen::Affine3d flangeHcalpattern_offset;

    offsets_->getFrame("flangeHcalpattern_offset",
                       flangeHcalpattern_offset_kdl);
    kdlToEigen3dPose(flangeHcalpattern_offset_kdl, flangeHcalpattern_offset);

    // robotbaseHcamera offset
    KDL::Frame robotbaseHcamera_offset_kdl;
    Eigen::Affine3d robotbaseHcamera_offset;

    char id[256];
    sprintf(id, "robotbaseHcamera_offset_%d", data_.calibration_run);

    offsets_->getFrame(id, robotbaseHcamera_offset_kdl);
    kdlToEigen3dPose(robotbaseHcamera_offset_kdl, robotbaseHcamera_offset);

    // robotbaseHcamera: estimation x offset
    Eigen::Affine3d robotbaseHcamera_1 =
        data_.robotbaseHcamera * robotbaseHcamera_offset;

    // robotbaseHcamera: through robot
    Eigen::Affine3d robotbaseHcamera_2 = arm_pose * data_.flangeHcalpattern *
                                         flangeHcalpattern_offset *
                                         data_.cameraHcalpattern.inverse();

    // Translation error
    Eigen::Vector3d t_error =
        robotbaseHcamera_1.translation() - robotbaseHcamera_2.translation();

    // rotation error
    Eigen::Matrix3d r_error =
        robotbaseHcamera_1.rotation().inverse() * robotbaseHcamera_2.rotation();

    // Compute residuals
    residuals[0] = static_cast<T>(t_error.x());
    residuals[1] = static_cast<T>(t_error.y());
    residuals[2] = static_cast<T>(t_error.z());

    Eigen::AngleAxisd aa(r_error);
    Eigen::Quaterniond q(aa);
    residuals[3] = static_cast<T>(q.x());
    residuals[4] = static_cast<T>(q.y());
    residuals[5] = static_cast<T>(q.z());
    residuals[6] = static_cast<T>(q.w());

    return true; // always return true
  }

update（）方法已重载以接受ceres :: Jet对象：

  bool update(const double *const free_params)
  {
    for (size_t i=0;i<parameter_offsets_.size();i++)
      parameter_offsets_[i] = free_params[i];
    return true;
  }

  bool update(const ceres::Jet<double,1> *const free_params)
  {
    for (size_t i=0; i<parameter_offsets_.size();i++)
      parameter_offsets_[i]=free_params[i].a;
    return true;
  }

我的感觉是，在优化过程中，参数不会在free_param对象中更新，因此它们对模型的影响（梯度范数）为空。

重载功能是否有误？我需要在update方法中管理喷射对象的v分量吗？

另外，错误可能是因为调用方法getChainFK()，但不涉及自由参数或残差...

如果需要的话，随时问我一些精度，如果您有任何建议...

感谢阅读！

问：Ceres Solver：如何使用DynamicAutoDiffCostFunctions？

0 个答案: