Go to the documentation of this file.00001 #ifndef CALIB_3D_CBS_H
00002 #define CALIB_3D_CBS_H
00003
00004 #include <gtsam/geometry/Pose3.h>
00005 #include <gtsam/geometry/Point3.h>
00006 #include <gtsam/geometry/Point2.h>
00007 #include <gtsam/geometry/SimpleCamera.h>
00008 #include <gtsam/nonlinear/Symbol.h>
00009 #include <gtsam/slam/PriorFactor.h>
00010 #include <gtsam/slam/ProjectionFactor.h>
00011 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
00012 #include <gtsam/nonlinear/DoglegOptimizer.h>
00013 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
00014 #include <gtsam/nonlinear/Values.h>
00015 #include <gtsam/linear/linearExceptions.h>
00016
00017 #include <boost/random/mersenne_twister.hpp>
00018 #include <boost/random/uniform_real.hpp>
00019 #include <boost/random/uniform_int.hpp>
00020 #include <boost/random/variate_generator.hpp>
00021 #include <boost/range/algorithm/random_shuffle.hpp>
00022
00023 #include <geometry_msgs/Pose.h>
00024
00025 #include <vector>
00026 #include <math.h>
00027
00028 using namespace std;
00029 using namespace gtsam;
00030
00031 namespace arm_3d_cb_calib {
00032
00034
00035 struct CBCalibProblem
00036 {
00037 vector<Pose3> base_T_ee_poses;
00038 vector<Point3> cb_p_points;
00039 vector<vector<vector<Point3> > > kinect_p_points;
00040 CBCalibProblem(int num_kinects, double cb_width, int num_cb_width, int num_cb_height) :
00041 kinect_p_points(num_kinects, vector<vector<Point3> >()) {
00042 for(int j=0;j<num_cb_height;j++)
00043 for(int i=0;i<num_cb_width;i++)
00044 cb_p_points.push_back(Point3(i*cb_width,j*cb_width,0));
00045 }
00046 };
00047
00048 struct CBCalibSolution
00049 {
00050 Pose3 cb_T_ee_pose;
00051 vector<Pose3> kinect_T_base_poses;
00052 };
00053
00054 void generateCBCalibProblem(CBCalibProblem& prob, CBCalibSolution& sol, int num_ees);
00055 void solveCBCalibProblem(const CBCalibProblem& prob, CBCalibSolution& sol);
00057
00058 geometry_msgs::Pose gtsamPose3ToGeomPose(const gtsam::Pose3& pose_in);
00059 gtsam::Pose3 geomPoseToGtsamPose3(const geometry_msgs::Pose& pose_in);
00060
00061 }
00062 #endif