Bundle adjustment with position constraints

[Grace]

Hi,

Any one can help on how to add position constraints to camera poses in bundle adjustment.

I would like to implement the following.

Below is the pseudo codes.

class BACostFunction { public: BACostFunction(const Eigen::Vector2d &point2D, const Camera &camera) : point2D_(point2D), camera_(camera) { } static ceres::CostFunction* create(const Eigen::Vector2d& point2D, const Camera &camera) { return (new ceres::AutoDiffCostFunction <BACostFunction,2, 3, 1, 1, 1, 3> (new BACostFunction(point2D,camera))); } template <typename T> bool operator()(const T* const rvec, const T* const tx, const T* const ty, const T* const tz, const T* const point3D, T* residuals) const {

..... reproject_error = re_project(camera_,current_pose,worldPt,reprojected_pixel); if(!reproject_error) { residuals[0] = T(0.0); residuals[1] = T(0.0); return true; } // Re-projection error. residuals[0] = (reprojected_pixel(0)-T(point2D_(0))); residuals[1] = (reprojected_pixel(1)-T(point2D_(1))); return true; } private: const Eigen::Vector2d &point2D_; const Camera &camera_; };

-----------------------------------------------------------
struct GPSCostFunction { GPSCostFunction(Eigen::Vector3d _gps_measurement, double _weight):gps_measurement(_gps_measurement),weight(_weight) { } static ceres::CostFunction* create(Eigen::Vector3d gps_measurement, double weight) { return (new ceres::AutoDiffCostFunction <GPSCostFunction, 3, 1, 1, 1> (new GPSCostFunction(gps_measurement,weight))); } template <typename T> bool operator()(const T* const tx, const T* const ty, const T* const tz, T* residuals) const { Eigen::Matrix<T, 3, 1> translation; translation(0)=tx[0]; translation(1)=ty[0]; translation(2)=tz[0]; Eigen::Matrix<T, 3, 1> residual_dist; residuals[0] = translation(0)-T(gps_measurement(0)); residuals[1] = translation(1)-T(gps_measurement(1)); residuals[2] = translation(2)-T(gps_measurement(2)); return true; } private: const Eigen::Vector3d &gps_measurement; const double &weight; };

 

-----------------------------------------------------------

 ceres::Problem problem;

 //add GPS pose constraints.for(size_t i=0; i<contrained_pose.size(); i++)

{ ceres::CostFunction* gps_cost_function = NULL; gps_cost_function = GPSCostFunction::create(non_ECEF_gps_pose[i],1.0); Eigen::Vector3d* _tvec = &translation_list[i]; double* _tx = _tvec->data(); double* _ty = _tx+1; double* _tz = _tx+2; problem.AddResidualBlock(gps_cost_function,NULL,_tx,_ty,_tz);

}

--------------------------------------------

for(size_t point_id = 0; point_id < 2d_img_points.size(); point_id++)

{

ceres::CostFunction* cost_function = NULL; cost_function = BACostFunction::create(...); Eigen::Vector3d* _tvec = &translation_list[related_id];

double* _tx = _tvec->data(); double* _ty = _tx+1;

double* _tz = _tx+2;

ceres::CostFunction* cost_function = NULL;

problem.AddResidualBlock(cost_function,NULL,rvec_data,tx,ty,tz,threeD_point->data());

 

}

ceres::Solver::Options solver_options; solver_options.minimizer_progress_to_stdout = true; ceres::Solver::Summary summary; ceres::Solve(solver_options, &problem, &summary); std::cout << summary.FullReport() << "\n";

I got correct results when I just added BACostFunction for bundle adjustment.

I cannot get correct results (means the camera poses are not constrained to the GPS poses which already applied transformation) when I added the BACostFunction and GPSCostFunction.

Anyone can enlighten what is wrong.

Thanks,

Grace

[Pierre Moulon]

Hi Grace,

Code looks fine, but I would ask a question about what it modelized by translation_list?

Camera centers C or camera translations t?

Since C = -Rt you must be sure to use the right translations.

Since your GPS modelize the GPS camera center you must compare it to camera centers (camera C, and not camera t).

That's perhaps the source of the problem (a missing conversion from t to C in the gps distance functor).

Regards/Cordialement,

Pierre M

--
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/d4cbc90b-6253-47fd-8f03-1de15a99cafe%40googlegroups.com.
For more options, visit https://groups.google.com/d/optout.