Method operator()

tinynav/cpp/bundle_adjustment.cpp:31–44

        : observed_keypoint_(observed_keypoint), K_(K) {}
    template<typename T>
    bool operator()(const T* camera_parameters, const T* point_parameters, T* residuals) const {
        using Vector3T = Eigen::Matrix<T, 3, 1>;
        Eigen::Map<const Eigen::Matrix<T, 6, 1>> camera(camera_parameters);
        Eigen::Map<const Vector3T> point_3d(point_parameters);
        Eigen::Matrix<T, 3, 1> phi = Eigen::Map<const Eigen::Matrix<T, 3, 1>>(camera.data() + 3);
        Eigen::Matrix<T, 3, 1> translation = Eigen::Map<const Vector3T>(camera.data());
        Eigen::Matrix<T, 3, 3> rotation;
        ceres::AngleAxisToRotationMatrix(phi.data(), rotation.data());
        Vector3T point_3d_in_camera = rotation.transpose() * (point_3d - translation);
        Vector3T reprojection = K_ * point_3d_in_camera;
        residuals[0] = reprojection[0] / reprojection[2] - observed_keypoint_[0];
        residuals[1] = reprojection[1] / reprojection[2] - observed_keypoint_[1];
        return true;
    }
    private:
    Eigen::Vector2d observed_keypoint_;
    Eigen::Matrix3d K_;