Ceres AutoDiff: бесплатные параметры не обновляются при оптимизации, как это исправить? - PullRequest
Новая
       79

Ceres AutoDiff: бесплатные параметры не обновляются при оптимизации, как это исправить?

0 голосов
/ 25 сентября 2019

В настоящее время я работаю над пакетом ros, посвященным калибровке роботов, используя Ceres для оптимизации моделей.По сути, этот пакет предназначен для замыкания геометрической петли, включающей любого робота и 6d-датчик, вставляя параметры оптимизации между каждым звеном робота.Для N-осевого робота параметры делятся на N * 2 блоков параметров, разделяя каждую ссылку на два блока, один для перевода (x, y, z), один для ориентации (qx, qy, qz, qw).Локальная параметризация EigenQuaternionParameterization применяется к каждому блоку ориентации.

Я успешно использую его, используя числовое дифференцирование, и теперь пытаюсь использовать DynamicAutoDiffCostFunction, но кажется, что вычисление / вычисление Якобиана не работает правильно.

Вот сообщение о завершении, которое я получил после одной уникальной итерации: 

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

Вот мой оператор (): 

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 () был перегружен вaccept ceres :: Jet objects: 

  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 в процессе оптимизации, поэтому их влияние на модель (норма градиента) равно нулю.

Является ли функция перегрузки ошибкой?Нужно ли управлять компонентом v объекта jet в методе update?

Кроме того, ошибкой может быть вызов методов как getChainFK(), но так как он не включает в себя свободные параметры или остатки ...

Не стесняйтесь задавать мне некоторую точность, если это необходимо, иесли у вас есть какие-либо советы ...

Спасибо за чтение!

...