00001 /* 00002 * Copyright (C) 2014 Fetch Robotics Inc. 00003 * Copyright (C) 2013-2014 Unbounded Robotics Inc. 00004 * 00005 * Licensed under the Apache License, Version 2.0 (the "License"); 00006 * you may not use this file except in compliance with the License. 00007 * You may obtain a copy of the License at 00008 * 00009 * http://www.apache.org/licenses/LICENSE-2.0 00010 * 00011 * Unless required by applicable law or agreed to in writing, software 00012 * distributed under the License is distributed on an "AS IS" BASIS, 00013 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00014 * See the License for the specific language governing permissions and 00015 * limitations under the License. 00016 */ 00017 00018 // Author: Michael Ferguson 00019 00020 #ifndef ROBOT_CALIBRATION_CERES_OPTIMIZER_H 00021 #define ROBOT_CALIBRATION_CERES_OPTIMIZER_H 00022 00023 #include <ceres/ceres.h> 00024 00025 #include <urdf/model.h> 00026 #include <kdl_parser/kdl_parser.hpp> 00027 #include <robot_calibration_msgs/CalibrationData.h> 00028 00029 #include <robot_calibration/calibration_offset_parser.h> 00030 #include <robot_calibration/ceres/optimization_params.h> 00031 #include <robot_calibration/ceres/camera3d_to_arm_error.h> 00032 #include <robot_calibration/ceres/ground_plane_error.h> 00033 #include <robot_calibration/ceres/camera_to_camera_error.h> 00034 #include <robot_calibration/ceres/data_functions.h> 00035 #include <robot_calibration/ceres/outrageous_error.h> 00036 #include <robot_calibration/models/camera3d.h> 00037 #include <robot_calibration/models/chain.h> 00038 #include <boost/shared_ptr.hpp> 00039 #include <string> 00040 #include <map> 00041 00042 namespace robot_calibration 00043 { 00044 00046 class Optimizer 00047 { 00048 public: 00050 Optimizer(const std::string& robot_description); 00051 virtual ~Optimizer(); 00052 00060 int optimize(OptimizationParams& params, 00061 std::vector<robot_calibration_msgs::CalibrationData> data, 00062 bool progress_to_stdout = false); 00063 00065 boost::shared_ptr<ceres::Solver::Summary> summary() 00066 { 00067 return summary_; 00068 } 00069 00070 boost::shared_ptr<CalibrationOffsetParser> getOffsets() 00071 { 00072 return offsets_; 00073 } 00074 00075 private: 00076 urdf::Model model_; 00077 std::string root_frame_; 00078 std::string led_frame_; 00079 KDL::Tree tree_; 00080 00081 std::map<std::string, ChainModel*> models_; 00082 00083 boost::shared_ptr<CalibrationOffsetParser> offsets_; 00084 boost::shared_ptr<ceres::Solver::Summary> summary_; 00085 }; 00086 00087 } // namespace robot_calibration 00088 00089 #endif // ROBOT_CALIBRATION_CERES_OPTIMIZER_H