Program Listing for File outrageous_error.hpp
↰ Return to documentation for file (/tmp/ws/src/robot_calibration/robot_calibration/include/robot_calibration/cost_functions/outrageous_error.hpp
)
/*
* Copyright (C) 2014 Unbounded Robotics Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
// Author: Michael Ferguson
#ifndef ROBOT_CALIBRATION_COST_FUNCTIONS_OUTRAGEOUS_ERROR_HPP
#define ROBOT_CALIBRATION_COST_FUNCTIONS_OUTRAGEOUS_ERROR_HPP
#include <string>
#include <ceres/ceres.h>
#include <robot_calibration/models/chain3d.hpp>
#include <robot_calibration/optimization/offsets.hpp>
namespace robot_calibration
{
struct OutrageousError
{
OutrageousError(OptimizationOffsets* offsets,
std::string name,
double joint_scaling,
double position_scaling,
double rotation_scaling) :
offsets_(offsets),
name_(name),
joint_(joint_scaling),
position_(position_scaling),
rotation_(rotation_scaling)
{
}
virtual ~OutrageousError() {}
bool operator()(double const * const * free_params,
double * residuals) const
{
// Update calibration offsets based on free params
offsets_->update(free_params[0]);
residuals[0] = joint_ * offsets_->get(name_);
KDL::Frame f;
if (offsets_->getFrame(name_, f))
{
residuals[1] = position_ * f.p.x();
residuals[2] = position_ * f.p.y();
residuals[3] = position_ * f.p.z();
double x, y, z;
axis_magnitude_from_rotation(f.M, x, y, z);
residuals[4] = rotation_ * fabs(x);
residuals[5] = rotation_ * fabs(y);
residuals[6] = rotation_ * fabs(z);
}
else
{
residuals[1] = 0.0;
residuals[2] = 0.0;
residuals[3] = 0.0;
residuals[4] = 0.0;
residuals[5] = 0.0;
residuals[6] = 0.0;
}
return true;
}
static ceres::CostFunction* Create(OptimizationOffsets* offsets,
std::string name,
double joint_scaling,
double position_scaling,
double rotation_scaling)
{
ceres::DynamicNumericDiffCostFunction<OutrageousError, ceres::CENTRAL> * func;
func = new ceres::DynamicNumericDiffCostFunction<OutrageousError, ceres::CENTRAL>(
new OutrageousError(offsets, name, joint_scaling, position_scaling, rotation_scaling));
func->AddParameterBlock(offsets->size());
func->SetNumResiduals(7); // joint + 3 position + 3 rotation
return static_cast<ceres::CostFunction*>(func);
}
OptimizationOffsets * offsets_;
std::string name_;
double joint_;
double position_;
double rotation_;
};
} // namespace robot_calibration
#endif // ROBOT_CALIBRATION_COST_FUNCTIONS_OUTRAGEOUS_ERROR_HPP