From 4aae53e8ab1beaca09dc58b7e4280aba9b1490c3 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 5 Jul 2016 00:44:57 +0200 Subject: [PATCH] Added the fixed camera parameters and camera parameter optimizatioN --- .../include/strands_sweep_registration/pair3DError.h | 3 ++- strands_sweep_registration/src/ProblemFrameConnection.cpp | 6 ++++-- strands_sweep_registration/src/RobotContainer.cpp | 8 +++++++- strands_sweep_registration/src/pair3DError.cpp | 8 +++++--- 4 files changed, 18 insertions(+), 7 deletions(-) diff --git a/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h b/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h index 9aae4815..238ce2cd 100644 --- a/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h +++ b/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h @@ -22,6 +22,7 @@ using ceres::Solve; class pair3DError : public SizedCostFunction<3, 6, 6, 4> { public: + static bool optimizeCameraParams; pair3DError(double sw, double sh, double sz,double dw, double dh, double dz, double weight); ~pair3DError(); bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const ; @@ -34,7 +35,7 @@ class pair3DError : public SizedCostFunction<3, 6, 6, 4> { double dh; double dz; double weight; - bool optimizeCameraParams; + //bool optimizeCameraParams; double information; }; #endif diff --git a/strands_sweep_registration/src/ProblemFrameConnection.cpp b/strands_sweep_registration/src/ProblemFrameConnection.cpp index 84c5b006..b9194654 100644 --- a/strands_sweep_registration/src/ProblemFrameConnection.cpp +++ b/strands_sweep_registration/src/ProblemFrameConnection.cpp @@ -30,8 +30,10 @@ void ProblemFrameConnection::addMatchesToProblem(ceres::Problem & problem, float double sz = src->keypoint_depth.at(src_kp_id); double dz = dst->keypoint_depth.at(dst_kp_id); - CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight); - problem.AddResidualBlock(err, NULL, src_variable, dst_variable, params); + //CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight/pow(sz*sz+dz*dz,2)); + CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight); + problem.AddResidualBlock(err, 0, src_variable, dst_variable, params); + problem.AddResidualBlock(err, new ceres::HuberLoss(0.1), src_variable, dst_variable, params); } } diff --git a/strands_sweep_registration/src/RobotContainer.cpp b/strands_sweep_registration/src/RobotContainer.cpp index 41a27a37..39786904 100644 --- a/strands_sweep_registration/src/RobotContainer.cpp +++ b/strands_sweep_registration/src/RobotContainer.cpp @@ -118,6 +118,10 @@ RobotContainer::~RobotContainer(){ void RobotContainer::initializeCamera(double fx, double fy, double cx, double cy, unsigned int w, unsigned int h) { + fx = 535; + fy = fx; + cx = double(w-1.0)/2.0; + cy = double(h-1.0)/2.0; std::cout<<"Initializing camera with parameters "< getMatchesRansac(std::vector< ProblemFrameConnection * > & pc_vec, float weight = 1, float threshold = 0.005, int ransac_iter = 200000, int nr_points = 3){ +std::vector< CostFunction * > getMatchesRansac(std::vector< ProblemFrameConnection * > & pc_vec, float weight = 1, float threshold = 0.015, int ransac_iter = 200000, int nr_points = 3){ std::vector owner; std::vector match_id; std::vector< Eigen::Vector3f > src_points; @@ -430,6 +434,8 @@ std::vector RobotContainer::runInitialTraining(){ } } + Solve(options, &problem, &summary); + pair3DError::optimizeCameraParams = true; Solve(options, &problem, &summary); //std::cout << summary.FullReport() << "\n"; diff --git a/strands_sweep_registration/src/pair3DError.cpp b/strands_sweep_registration/src/pair3DError.cpp index 99532379..05d26e32 100644 --- a/strands_sweep_registration/src/pair3DError.cpp +++ b/strands_sweep_registration/src/pair3DError.cpp @@ -3,10 +3,12 @@ int sumid = 0; +bool pair3DError::optimizeCameraParams = false; + pair3DError::pair3DError(double sw, double sh, double sz,double dw, double dh, double dz, double weight) : sw(sw), sh(sh), sz(sz), dw(dw), dh(dh), dz(dz), weight(weight) \ { id = sumid++; - optimizeCameraParams = false; + //optimizeCameraParams = true; information = 1.0/1.5; } @@ -42,8 +44,8 @@ bool pair3DError::Evaluate(double const* const* parameters, double* residuals, d double cy = CameraParameters::get().cy(); if(optimizeCameraParams){ - invfx = params[0]; - invfy = params[1]; + invfx = fabs(params[0]); + invfy = fabs(params[1]); cx = params[2]; cy = params[3]; }