Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- struct RcpAndFpOptimizer {
- RcpAndFpOptimizer(cv::Mat &V, const cv::Mat I, int i,int j,double width, double height) : V_(V), I_(I), i_(i),
- j_(j), width_(width), height_(height){}
- bool operator()(const double* const fp, const double* const rotation, const double* const translation, double* residuals) const {
- double intensity = V_.at<double>(j_, i_);
- double tmp = (double)I_.at<double>(j_,i_)-(double)intensity;
- residuals[0] = tmp;
- //std::cout<<"pixels(i,j): "<<i_<<" "<<j_<<" residual: "<<residuals[0]<<std::endl;
- return true;
- }
- const cv::Mat S_;
- cv::Mat& V_;
- const cv::Mat I_;
- const int i_,j_;
- double width_, height_;
- };
- virtual void PrepareForEvaluation(bool evaluateJacobians, bool newEvaluationPoint)
- {
- if(evaluateJacobians){
- std::cout<<"evaluation jacobian is called"<<std::endl;
- }
- if (newEvaluationPoint)
- {
- // do your stuff here, e.g. calculate integral image
- //Mat V(height_, width_, CV_8UC1);
- std::cout<<"preperation is called"<<std::endl;
- Intrinsic<double> intrinsicC = INTRINSIC_CAMERA;
- Intrinsic<double> intrinsicP= {(double)fP_[0],(double)fP_[0], double(width_/2), double(height_/2), 0, 0};
- //Convertion of array to point3d
- Point3d bDist = Point3d(translation_[0],translation_[1], translation_[2]);
- //Convertion euler array to rotation matrix
- const Mat eulerAngles = (cv::Mat_<double>(3,1) << rotArray_[0], rotArray_[1], rotArray_[2]);
- Mat rotM = rcpFinder::euler2rot(eulerAngles);
- Mat tempVImg(height_, width_, CV_8UC1);
- for (int i = 0; i < width_; ++i) {
- for (int j = 0; j < height_ ; ++j) {
- //std::cout<<"Virtual current x and y pixels: "<<i<<" "<<j<<std::endl;
- Point3d unprojPRay = rcpFinder::unprojectPoints(Point2i(i,j),intrinsicC);
- //Assigning the intensity from images
- tempVImg.at<uchar>(j, i)= rcpFinder::genVirtualImg(S_, intrinsicP, bDist, unprojPRay,
- planeNormalAndDistance_, rotM);
- auto pixelIntensity = tempVImg.at<uchar>(Point(j, i));
- //std::cout<<"pixel intensity "<< pixelIntensity<<std::endl;
- }
- }
- //imshow("Virtual", tempVImg);
- Mat integralV;
- cv::integral(tempVImg, integralV);
- //std::cout<<"integral image type is "<<integralV.type()<<std::endl;
- rcpFinder::normalizePixelsImg(tempVImg, integralV, V_);
- /*imshow("Normalized Img", V_);
- waitKey(0);*/
- }
- }
- // stuff here
- const cv::Mat S_;
- cv::Mat& V_;
- int width_, height_;
- map<int, vector<Point3d>> planeNormalAndDistance_;
- double *translation_;
- double* rotArray_;
- double* fP_;
- };
- cv::Mat integralImgI;
- cv::integral(im1, integralImgI);
- cv::Mat normalizedRealImg;
- rcpFinder::normalizePixelsImg(im1, integralImgI, normalizedRealImg);
- Mat normalizedVirtualImg;
- //ceres::CostFunction* total_cost_function = 0;
- for (int i = 1; i < width-1; ++i) {
- for (int j = 1; j < height-1 ; ++j) {
- ceres::CostFunction* cost_function =
- new ceres::NumericDiffCostFunction<RcpAndFpOptimizer, ceres::CENTRAL, 1, 1, 3, 3>(
- new RcpAndFpOptimizer(normalizedVirtualImg, normalizedRealImg, i, j, width, height));
- problem.AddResidualBlock(cost_function, NULL, fp, rotationArray, translation);
- }
- }
- ceres::Solver::Options options;
- options.minimizer_progress_to_stdout = true;
- options.max_num_iterations = 50;
- options.update_state_every_iteration = true;
- options.evaluation_callback = (new evaluation_callback_functor(S, normalizedVirtualImg,width, height,
- mapNormalAndDist, translation,rotationArray, fp));
- ceres::Solver::Summary summary;
- ceres::Solve(options, &problem, &summary);
- std::cout << summary.BriefReport() << "n";
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement