Added the fixed camera parameters and camera parameter optimizatioN

strands-project · Jul 4, 2016 · 4aae53e · 4aae53e
1 parent 1462f9e
commit 4aae53e
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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 "<<fx<<"  "<<fy<<"  "<<cx<<"  "<<cy<<"  "<<w<<"  "<<h<<std::endl;
     width = w;
     height = h;
@@ -236,7 +240,7 @@ void RobotContainer::addToTraining(std::string path){
     */
 }
 
-std::vector< CostFunction * > 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<int> owner;
     std::vector<int> match_id;
     std::vector< Eigen::Vector3f > src_points;
@@ -430,6 +434,8 @@ std::vector<Eigen::Matrix4f> 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
@@ -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];
     }