// C = inv(R) * -t;

gistfile1.txt

struct CameraPositionError : public AbstractPositionError {
    CameraPositionError(vector<double> &observed, double sigma)
        : observed(observed), sigma(sigma) {
        weigth = 1 / (sigma * sigma);
    }

    vector<double> observed;
    double sigma, weigth;

    template <typename T>
    bool operator()(const T* const cam_ex, const T* const cam_offset, const T* const gps_gcp_offset,  T* residuals) const {
        T cam_center[3], gps_center[3], obs[3], cam_ex_inv[3], offset[3];

        // C = inv(R) * -t;
        cam_ex_inv[0] = T(-1) * cam_ex[0];
        cam_ex_inv[1] = T(-1) * cam_ex[1];
        cam_ex_inv[2] = T(-1) * cam_ex[2];
        ceres::AngleAxisRotatePoint(cam_ex_inv, &cam_ex[3], cam_center);
        cam_center[0] *= T(-1);
        cam_center[1] *= T(-1);
        cam_center[2] *= T(-1);


        // C_gps = C_camera + C_offset
        offset[0] = T(cam_offset[0]); //cam_offset[0];
        offset[1] = T(-1) * T(cam_offset[1]); //cam_offset[1];
        offset[2] = T(-1) * T(cam_offset[2]); //cam_offset[2];

        ceres::AngleAxisRotatePoint(cam_ex_inv, offset, gps_center);
        gps_center[0] += cam_center[0];
        gps_center[1] += cam_center[1];
        gps_center[2] += cam_center[2];

        obs[0] = T(observed[0]) + gps_gcp_offset[0];
        obs[1] = T(observed[1]) + gps_gcp_offset[1];
        obs[2] = T(observed[2]) + gps_gcp_offset[2];

        bool ret = residualError(gps_center, obs, residuals);

        residuals[0] *= T(weigth);
        residuals[1] *= T(weigth);
        residuals[2] *= T(weigth);

        return ret;
    }

    static ceres::CostFunction* Create(vector<double> &pos, double sigma) {
        return new ceres::AutoDiffCostFunction<CameraPositionError, 3, 6, 3, 3>(new CameraPositionError(pos, sigma));
    }
};