Problem with Using DynamicCostFunctionToFunctor

[Ginger C.]

Hi everyone， 

I have a simple example, based upon http://ceres-solver.org/nnls_modeling.html?highlight=dynamiccostfunctiontofunctor#dynamiccostfunctiontofunctor ,

There are some Camera pose [allPose] and 3D point position [vpMPPos]  I want to optimize by using BundleAdjustment.

DATA TYPE:

[allPose] is [std::vector<cv::Mat>] type and [vpMPPos] is [std::vector<cv::Point3d>] type.

In the [allPose], every member is a 6-dim vector consisting of rotation and translation vectors. And in the [vpMPPos], every member is 3-dim vector

In my code, I use [DynamicCostFunctionToFunctor] to compute Residual, before compute residual, I use ThirdPartyFunction: [ProjectPoint()] to complete projection 

ERROR while running the program(see picture)

The ERROR appears the value of the Parameter Block 0 is 0, Parameter Block 1 is not 0,  

And Jacobian is " Not Computed" 

but I can't see what is the issue in my code :(

THE CODE:

// allPose is std::vector<cv::Mat>
// vpMPPos is std::vector<cv::Point3d>
std::vector<double*> param_blocks;
param_blocks.push_back(allPose[i].ptr<double>());
param_blocks.push_back(&(vpMPPos[j].x));

DynamicCeresBA::DynamicCeresBACostFunction* cost_function =
DynamicCeresBA::Create(pKF->fx, pKF->fy, pKF->cx, pKF->cy, pKF->mbf,
                                               p2d_lr.x, p2d_lr.y, p2d_lr.z);

problem.AddResidualBlock(cost_function,
                                                 loss_fuction,

                                                 param_blocks);
//-------------------------------------------------- ------------------------------------- -------------------------------------
void ProjectPoint(const double* rt, const double* p3d,
                                double fx, double fy, double cx, double cy, double bf,
                                double &projection_x, double &projection_y, double &projection_r_x){
     
       const double r1 = rt[0]; const double r2 = rt[1]; const double r3 = rt[2];
       const double t1 = rt[3]; const double t2 = rt[4]; const double t3 = rt[5];
       const double p1 = p3d[0]; const double p2 = p3d[1]; const double p3 = p3d[2];
     

       // use Eigen, Rotation vector to rotation matrix
       Eigen::AngleAxisd rvec(0, Eigen::Vector3d::UnitX());
       rvec.axis() = Eigen::Vector3d(r1, r2, r3);
       Eigen::Matrix3d Rotation = rvec.toRotationMatrix();

       // R * p + t  : world_coordinate -> cam_coordinate

       double p00, p01, p02;
       p00 = Rotation(0, 0) * p1 + Rotation(0, 1) * p2 + Rotation(0, 2) * p3;
       p01 = Rotation(1, 0) * p1 + Rotation(1, 1) * p2 + Rotation(1, 2) * p3;
       p02 = Rotation(2, 0) * p1 + Rotation(2, 1) * p2 + Rotation(2, 2) * p3;

       p00 += t1;
       p01 += t2;
       p02 += t3;
      

       // cam_coordinate to image_coordinate include Left and Right image
       projection_x = fx * p00 / p02 + cx;
       projection_y = fy * p01 / p02 + cy;
       projection_r_x = projection_x - bf / p02;
}

struct DynamicGetRtByEPnP
{

         // construct
         DynamicGetRtByEPnP(double fx, double fy, double cx, double cy, double bf,
                                                   double observation_x, double observation_y, double observation_r_x)

              : fx_(fx), fy_(fy), cx_(cx), cy_(cy), bf_(bf),

              observation_x_(observation_x), observation_y_(observation_y),                                                observation_r_x_(observation_r_x) { }

     bool operator() (double const* const* parameters,

                                double* residuals) const {

                  double projection_x;
                  double projection_y;
                  double projection_r_x;

                  const double* rt = parameters[0];
                  const double* p3d = parameters[1];
                  // ThirdPartyFunction
                  ProjectPoint(rt, p3d,
                  fx_, fy_, cx_, cy_, bf_,
                  projection_x, projection_y, projection_r_x);

                  residuals[0] = observation_x_ - projection_x;
                  residuals[1] = observation_y_ - projection_y;
                  residuals[1] = observation_r_x_ - projection_r_x;
         return true;
}

          double fx_, fy_, cx_, cy_, bf_;
         double observation_x_, observation_y_, observation_r_x_;
};

// Solve Cost_Function
struct DynamicCeresBA {

                  typedef ceres::DynamicAutoDiffCostFunction<DynamicCeresBA, 4>                 DynamicCeresBACostFunction;

               

                 // construct
                  DynamicCeresBA(double fx, double fy, double cx, double cy, double bf,
                                  double observation_x, double observation_y, double observation_r_x)

{
                   ceres::DynamicNumericDiffCostFunction<DynamicGetRtByEPnP> *cost_funtion
= new ceres::DynamicNumericDiffCostFunction<DynamicGetRtByEPnP>(
new DynamicGetRtByEPnP(fx, fy, cx, cy, bf, observation_x, observation_y, observation_r_x));

                   cost_funtion->AddParameterBlock(6); // R t
                   cost_funtion->AddParameterBlock(3); // point
                   cost_funtion->SetNumResiduals(3);

                   compute_PointRt_.reset(new ceres::DynamicCostFunctionToFunctor(cost_funtion));

}

template <typename T>
bool operator()(T const* const* parameters,
                            T* residuals) const {

                  return (*compute_PointRt_)(parameters, residuals);
}

// creat a costfunction from DynamicCeresBA to add to a ceres problem
static DynamicCeresBACostFunction* Create(double fx, double fy, double cx, double cy, double bf, double point2d_x, double point2d_y, double point2d_r_x) {

// DynamicAutoDiffCostFunction
DynamicCeresBA* constraint = new DynamicCeresBA(fx, fy, cx, cy, bf, point2d_x,                                    point2d_y, point2d_r_x);

                   DynamicCeresBACostFunction* cost_function = new                  DynamicCeresBACostFunction(constraint);

                   cost_function->AddParameterBlock(6); // Rt
                   cost_function->AddParameterBlock(3); // point

                   cost_function->SetNumResiduals(3);

                   return (cost_function);
}

private:
                   std::unique_ptr<ceres::DynamicCostFunctionToFunctor> compute_PointRt_;

};

[Dmitriy Korchemkin]

> residuals[1] = observation_y_ - projection_y;

> residuals[1] = observation_r_x_ - projection_r_x;

You are setting residuals[1] twice.

You might benefit from using library similar to Eigen (in ability to map data structures over raw pointers) in order to reduce number of errors of that type.

--
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/d4003b17-9c24-493c-9d77-a121494d06f0n%40googlegroups.com.