outrageous_error.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Unbounded Robotics Inc.
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 // Author: Michael Ferguson
18 
19 #ifndef ROBOT_CALIBRATION_CERES_OUTRAGEOUS_ERROR_H
20 #define ROBOT_CALIBRATION_CERES_OUTRAGEOUS_ERROR_H
21 
22 #include <string>
23 #include <ceres/ceres.h>
26 
27 namespace robot_calibration
28 {
29 
35 {
47  std::string name,
48  double joint_scaling,
49  double position_scaling,
50  double rotation_scaling) :
51  offsets_(offsets),
52  name_(name),
53  joint_(joint_scaling),
54  position_(position_scaling),
55  rotation_(rotation_scaling)
56  {
57  }
58 
59  virtual ~OutrageousError() {}
60 
66  bool operator()(double const * const * free_params,
67  double * residuals) const
68  {
69  // Update calibration offsets based on free params
70  offsets_->update(free_params[0]);
71 
72  residuals[0] = joint_ * offsets_->get(name_);
73  KDL::Frame f;
74  if (offsets_->getFrame(name_, f))
75  {
76  residuals[1] = position_ * f.p.x();
77  residuals[2] = position_ * f.p.y();
78  residuals[3] = position_ * f.p.z();
79  double x, y, z;
80  axis_magnitude_from_rotation(f.M, x, y, z);
81  residuals[4] = rotation_ * fabs(x);
82  residuals[5] = rotation_ * fabs(y);
83  residuals[6] = rotation_ * fabs(z);
84  }
85  else
86  {
87  residuals[1] = 0.0;
88  residuals[2] = 0.0;
89  residuals[3] = 0.0;
90  residuals[4] = 0.0;
91  residuals[5] = 0.0;
92  residuals[6] = 0.0;
93  }
94 
95  return true;
96  }
97 
101  static ceres::CostFunction* Create(CalibrationOffsetParser* offsets,
102  std::string name,
103  double joint_scaling,
104  double position_scaling,
105  double rotation_scaling)
106  {
107  ceres::DynamicNumericDiffCostFunction<OutrageousError, ceres::CENTRAL> * func;
108  func = new ceres::DynamicNumericDiffCostFunction<OutrageousError, ceres::CENTRAL>(
109  new OutrageousError(offsets, name, joint_scaling, position_scaling, rotation_scaling));
110  func->AddParameterBlock(offsets->size());
111  func->SetNumResiduals(7); // joint + 3 position + 3 rotation
112  return static_cast<ceres::CostFunction*>(func);
113  }
114 
116  std::string name_;
117  double joint_;
118  double position_;
119  double rotation_;
120 };
121 
122 } // namespace robot_calibration
123 
124 #endif // ROBOT_CALIBRATION_CERES_OUTRAGEOUS_ERROR_H
Error block to restrict the offsets generated by ceres-solver from becoming outrageously large...
f
CalibrationOffsetParser * offsets_
bool getFrame(const std::string name, KDL::Frame &offset) const
Get the offset for a frame calibration.
OutrageousError(CalibrationOffsetParser *offsets, std::string name, double joint_scaling, double position_scaling, double rotation_scaling)
Constructor for an error functor.
Combined parser and configuration for calibration offsets. Holds the configuration of what is to be c...
TFSIMD_FORCE_INLINE const tfScalar & y() const
double z() const
Rotation M
double y() const
double x() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
Calibration code lives under this namespace.
double get(const std::string name) const
Get the offset.
void axis_magnitude_from_rotation(const KDL::Rotation &r, double &x, double &y, double &z)
Converts from KDL::Rotation to angle-axis-with-integrated-magnitude.
Definition: models.cpp:261
TFSIMD_FORCE_INLINE const tfScalar & z() const
static ceres::CostFunction * Create(CalibrationOffsetParser *offsets, std::string name, double joint_scaling, double position_scaling, double rotation_scaling)
Helper factory function to create a new error block.
bool update(const double *const free_params)
Update the offsets based on free_params from ceres-solver.
bool operator()(double const *const *free_params, double *residuals) const
Operator for ceres-solver to call.


robot_calibration
Author(s): Michael Ferguson
autogenerated on Tue Nov 3 2020 17:30:30