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/data_functions.h> 00034 #include <robot_calibration/ceres/outrageous_error.h> 00035 #include <robot_calibration/models/camera3d.h> 00036 #include <robot_calibration/models/chain.h> 00037 #include <boost/shared_ptr.hpp> 00038 #include <string> 00039 #include <map> 00040 00041 namespace robot_calibration 00042 { 00043 00045 class Optimizer 00046 { 00047 public: 00049 Optimizer(const std::string& robot_description); 00050 virtual ~Optimizer(); 00051 00059 int optimize(OptimizationParams& params, 00060 std::vector<robot_calibration_msgs::CalibrationData> data, 00061 bool progress_to_stdout = false); 00062 00064 boost::shared_ptr<ceres::Solver::Summary> summary() 00065 { 00066 return summary_; 00067 } 00068 00069 boost::shared_ptr<CalibrationOffsetParser> getOffsets() 00070 { 00071 return offsets_; 00072 } 00073 00074 private: 00075 urdf::Model model_; 00076 std::string root_frame_; 00077 std::string led_frame_; 00078 KDL::Tree tree_; 00079 00080 std::map<std::string, ChainModel*> models_; 00081 00082 boost::shared_ptr<CalibrationOffsetParser> offsets_; 00083 boost::shared_ptr<ceres::Solver::Summary> summary_; 00084 }; 00085 00086 } // namespace robot_calibration 00087 00088 #endif // ROBOT_CALIBRATION_CERES_OPTIMIZER_H