Re: [ceres-solver] RGBD Bundle adjustment doesn't optimize

[Sameer Agarwal]

Clearly your objective function is getting reduced and the reconstruction is changing shape.

So the problem is not with ceres, but rather with the mathematical formulation of your objective function.

I recommend taking a closer look at your mathematical formulation.

Sameer

On Wed, Apr 28, 2021 at 3:53 PM f2020 l_ <lanfeiy...@gmail.com> wrote:

Hi,

I am using ceres solver for RGBD direct bundle adjustment. I am using Sophus for the camera pose. I implemented AutoDiff Local parameterization and cost functor. Then, I build the problem based on the tutorial.

Here is my code:

AutoDiffParameterization for SE3:

struct AutoDiffLocalParameterizationSE3

{

template<typename T>

bool operator()

(

const T* T_raw,

const T* delta_raw,

T* T_plus_delta_raw

)const

{

Eigen::Map<Sophus::SE3<T> const> pose_se3(T_raw);

Eigen::Map<Eigen::Matrix<T, 6, 1> const> delta(delta_raw);

Eigen::Map<Sophus::SE3<T>> T_plus_delta(T_plus_delta_raw);

T_plus_delta = pose_se3 * Sophus::SE3<T>::exp(delta);

return true;

}

static ceres::LocalParameterization* Create()

{

return new ceres::AutoDiffLocalParameterization<AutoDiffLocalParameterizationSE3, Sophus::SE3d::num_parameters, Sophus::SE3d::DoF>();

}

};

RGBD CostFunction:

struct RGBDCostFunctor

{

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

RGBDCostFunctor

(

const ceres::DepthBiCubicInterpolator<ceres::Grid2D<double, 1>>& kf_depth_interp,

const ceres::BiCubicInterpolator<ceres::Grid2D<double, 1>>& kf_intensity_interp,

const double surfel_intensity,

const Eigen::Matrix3d& intrinsics_mat

):

kf_depth_interp_(kf_depth_interp),

kf_intensity_interp_(kf_intensity_interp),

intrinsics_mat_(intrinsics_mat),

surfel_intensity_(surfel_intensity)

{

}

template<typename T>

bool operator()

(

const T* surfel,

const T* kf_pose_se3_raw,

T* out_residuals

) const

{

Eigen::Map<Eigen::Matrix<T, 3, 1> const> g_surfel(surfel);

Eigen::Map<Sophus::SE3<T> const> kf_pose_se3(kf_pose_se3_raw);

Eigen::Matrix<T, 3, 1> l_surfel = kf_pose_se3 * g_surfel;

Eigen::Matrix<T, 3, 1> pixel_coord = intrinsics_mat_ * l_surfel / l_surfel(2, 0);

T depth_in_kf;

bool is_success = kf_depth_interp_.Evaluate(pixel_coord(1, 0), pixel_coord(0, 0), &depth_in_kf);

if(is_success == false)

{

out_residuals[0] = T(0.0);

out_residuals[1] = T(0.0);

return true;

}

T depth_l_surfel = l_surfel(2, 0);

out_residuals[0] = depth_in_kf - depth_l_surfel;

T intensity_kf;

kf_intensity_interp_.Evaluate(pixel_coord(1, 0), pixel_coord(0, 0), &intensity_kf);

out_residuals[1] = surfel_intensity_ - intensity_kf;

return true;

}

static ceres::CostFunction* Create

(

const ceres::DepthBiCubicInterpolator<ceres::Grid2D<double, 1>>& kf_depth_interp,

const ceres::BiCubicInterpolator<ceres::Grid2D<double, 1>>& kf_intensity_interp,

const double surfel_intensity,

const Eigen::Matrix3d& intrinsics_mat

)

{

return

(

new ceres::AutoDiffCostFunction

<RGBDCostFunctor, 2, 3, Sophus::SE3d::num_parameters>

(new RGBDCostFunctor(kf_depth_interp, kf_intensity_interp, surfel_intensity, intrinsics_mat))

);

}

const ceres::DepthBiCubicInterpolator<ceres::Grid2D<double, 1>>& kf_depth_interp_;

const ceres::BiCubicInterpolator<ceres::Grid2D<double, 1>>& kf_intensity_interp_;

double surfel_intensity_;

Eigen::Matrix3d intrinsics_mat_;

};

Before bundle adjustment:

After bundle adjustment: 

The full report of ceres solver is:

The bundle adjustment doesn't optimize the model. It is even worse.

I am struggling with this problem. Could you tell me where I am wrong?

--
You received this message because you are subscribed to the Google Groups "Ceres Solver" group.
To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/CAEB%2BRJrno7qTZufyMKgQZSpBJ3%2B_iE0vt9ibQZyPZ%2BmGqzW7DQ%40mail.gmail.com.

[f2020 l_]

Hi,

The objective function is a 2D vector:

where r_g is the geometric error, and r_p is the photometric error. They are defined as below:

Where Z() is the depth image of the keyframe. 

The function pi() means projection. x_s is the model point. the twist is the pose. 

In the second term, {}_z  operator takes the z component of the point. 

This geometric error is:  transform the model point into the local camera space of the keyframe, then, compute the depth error between the local model point and the depth measurements.

The first term is the intensity of the model point x_s.

I_kf is the intensity image of the keyframe. The second term is the intensity of the model point projected into the keyframe. 

So, by this joint optimization, the geometry of the model shouldn't be changed.

I still cannot find where I was wrong. Could you help me? Thank you very much!

[f2020 l_]

The parameters are the poses of the keyframes and the model points.

Yes, the geometry of the model needs to be optimized. This is a joint optimization of keyframe poses and model points. 

Why the results are even worse? 

[Dmitriy Korchemkin]

Hi,

Does this part of code correspond to handling points that are projecting outside the image?

bool is_success = kf_depth_interp_.Evaluate(pixel_coord(1, 0), pixel_coord(0, 0), &depth_in_kf);

if(is_success == false)

{

    out_residuals[0] = T(0.0);

    out_residuals[1] = T(0.0);

    return true;

}

If it is the case - then moving cameras in a such way that more points are projected outside the image is very beneficial in terms of cost function value.

I suppose that this is the reason why you get diverged results while cost-function value decreases by a factor of 10.

Note that this also makes your cost-function non-continuous (and, thus, non-differentiable) when one of the points with non-zero residual leaves the frame.

[f2020 l_]

Hi,

That part of the code handles missing data of depth images. If the model point is projected into the missing data area, the cost function will give zero jacobians and zero residual. 

For the point outside the image, it will be evaluated using the boundary of depth images.  For example, 

x = min(  max(0, x),  image_width  );

y = min(  max(0, y),  image_height );

So I should modify my objective function? Or change my implementation of the cost function?

[f2020 l_]

Is the objective function wrong or the implementation?