Calibration code lives under this namespace. More...
Classes | |
class | CalibrationOffsetParser |
Combined parser and configuration for calibration offsets. Holds the configuration of what is to be calibrated, and and parses the actual adjustments from the free parameters. More... | |
class | Camera3dModel |
Model of a camera on a kinematic chain. More... | |
struct | Camera3dToArmError |
Model of a camera on a kinematic chain. More... | |
struct | CameraToCameraError |
Model of a camera on another camera chain. More... | |
class | ChainManager |
Manages moving joints to a new pose, determining when they are settled, and returning current joint_states. More... | |
class | ChainModel |
Model of a kinematic chain. This is the basic instance where we transform the world observations into the proper root frame. More... | |
class | CheckerboardFinder |
This class processes the point cloud input to find a checkerboard. More... | |
class | DepthCameraInfoManager |
Base class for a feature finder. More... | |
class | FeatureFinder |
Base class for a feature finder. More... | |
struct | GroundPlaneError |
class | GroundPlaneFinder |
class | LedFinder |
This class processes the point cloud input to find the LED. More... | |
struct | OptimizationParams |
Class to hold parameters for optimization. More... | |
class | Optimizer |
Class to do optimization. More... | |
struct | OutrageousError |
Error block to restrict the offsets generated by ceres-solver from becoming outrageously large. More... | |
class | PlaneFinder |
Typedefs | |
typedef std::map< std::string, FeatureFinderPtr > | FeatureFinderMap |
typedef boost::shared_ptr < FeatureFinder > | FeatureFinderPtr |
Enumerations | |
enum | { CAMERA_INFO_P_FX_INDEX = 0, CAMERA_INFO_P_FY_INDEX = 5, CAMERA_INFO_P_CX_INDEX = 2, CAMERA_INFO_P_CY_INDEX = 6 } |
enum | { CAMERA_INFO_K_FX_INDEX = 0, CAMERA_INFO_K_FY_INDEX = 4, CAMERA_INFO_K_CX_INDEX = 2, CAMERA_INFO_K_CY_INDEX = 5 } |
enum | { CAMERA_INFO_D_1 = 0, CAMERA_INFO_D_2 = 1, CAMERA_INFO_D_3 = 2, CAMERA_INFO_D_4 = 3, CAMERA_INFO_D_5 = 4 } |
enum | { CAMERA_PARAMS_CX_INDEX = 0, CAMERA_PARAMS_CY_INDEX = 1, CAMERA_PARAMS_FX_INDEX = 2, CAMERA_PARAMS_FY_INDEX = 3, CAMERA_PARAMS_Z_SCALE_INDEX = 4, CAMERA_PARAMS_Z_OFFSET_INDEX = 5 } |
Functions | |
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. | |
geometry_msgs::PointStamped | computeAverage (std::vector< geometry_msgs::PointStamped > points) |
Compute the average point based on a vector of points. | |
double | computeAverage (std::vector< double > values) |
Compute the average value based on a vector of doubles. | |
geometry_msgs::PointStamped | computeDifference (geometry_msgs::PointStamped &p1, geometry_msgs::PointStamped &p2) |
Compute the difference in between two points. Note that points must be in the same frame. | |
double | distancePoints (const geometry_msgs::Point p1, const geometry_msgs::Point p2) |
double | getDistance (geometry_msgs::PointStamped &p1, geometry_msgs::PointStamped &p2) |
Compute the distance in Euclidean space between two points. Note that points must be in the same frame. | |
std::vector < geometry_msgs::PointStamped > | getErrorPoints (ChainModel *chain1, ChainModel *chain2, CalibrationOffsetParser *offsets, robot_calibration_msgs::CalibrationData &data) |
Returns a vector of points representing the differences between two projections using different chains but the same calibration offsets. | |
std::vector< double > | getErrors (ChainModel *chain1, ChainModel *chain2, CalibrationOffsetParser *offsets, robot_calibration_msgs::CalibrationData &data) |
Returns the distance between each point in a given sample. | |
bool | hasSensor (const robot_calibration_msgs::CalibrationData &msg, const std::string &sensor) |
bool | loadFeatureFinders (ros::NodeHandle &nh, FeatureFinderMap &features) |
Load feature finders, based on param server config. | |
double | positionFromMsg (const std::string &name, const sensor_msgs::JointState &msg) |
void | printComparePoints (ChainModel *chain1, ChainModel *chain2, CalibrationOffsetParser *before, CalibrationOffsetParser *after, robot_calibration_msgs::CalibrationData &data) |
void | printComparePointsInternal (ChainModel *chain1, ChainModel *chain2, CalibrationOffsetParser *offsets, robot_calibration_msgs::CalibrationData &data) |
void | printSimpleDistanceError (ChainModel *chain1, ChainModel *chain2, CalibrationOffsetParser *before, CalibrationOffsetParser *after, robot_calibration_msgs::CalibrationData &data) |
KDL::Rotation | rotation_from_axis_magnitude (const double x, const double y, const double z) |
Converts our angle-axis-with-integrated-magnitude representation to a KDL::Rotation. | |
sensor_msgs::CameraInfo | updateCameraInfo (double camera_fx, double camera_fy, double camera_cx, double camera_cy, const sensor_msgs::CameraInfo &info) |
Update the camera calibration with the new offsets. | |
Variables | |
const unsigned | B = 2 |
const unsigned | G = 1 |
const unsigned | R = 0 |
const unsigned | X = 0 |
const unsigned | Y = 1 |
const unsigned | Z = 2 |
Calibration code lives under this namespace.
typedef std::map<std::string, FeatureFinderPtr > robot_calibration::FeatureFinderMap |
Definition at line 45 of file feature_finder.h.
typedef boost::shared_ptr<FeatureFinder> robot_calibration::FeatureFinderPtr |
Definition at line 44 of file feature_finder.h.
anonymous enum |
CAMERA_INFO_P_FX_INDEX | |
CAMERA_INFO_P_FY_INDEX | |
CAMERA_INFO_P_CX_INDEX | |
CAMERA_INFO_P_CY_INDEX |
Definition at line 29 of file camera_info.h.
anonymous enum |
CAMERA_INFO_K_FX_INDEX | |
CAMERA_INFO_K_FY_INDEX | |
CAMERA_INFO_K_CX_INDEX | |
CAMERA_INFO_K_CY_INDEX |
Definition at line 34 of file camera_info.h.
anonymous enum |
Definition at line 39 of file camera_info.h.
anonymous enum |
CAMERA_PARAMS_CX_INDEX | |
CAMERA_PARAMS_CY_INDEX | |
CAMERA_PARAMS_FX_INDEX | |
CAMERA_PARAMS_FY_INDEX | |
CAMERA_PARAMS_Z_SCALE_INDEX | |
CAMERA_PARAMS_Z_OFFSET_INDEX |
Definition at line 45 of file camera_info.h.
void robot_calibration::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 at line 248 of file models.cpp.
geometry_msgs::PointStamped robot_calibration::computeAverage | ( | std::vector< geometry_msgs::PointStamped > | points | ) |
Compute the average point based on a vector of points.
Definition at line 30 of file data_functions.h.
double robot_calibration::computeAverage | ( | std::vector< double > | values | ) |
Compute the average value based on a vector of doubles.
Definition at line 50 of file data_functions.h.
geometry_msgs::PointStamped robot_calibration::computeDifference | ( | geometry_msgs::PointStamped & | p1, |
geometry_msgs::PointStamped & | p2 | ||
) |
Compute the difference in between two points. Note that points must be in the same frame.
Definition at line 63 of file data_functions.h.
double robot_calibration::distancePoints | ( | const geometry_msgs::Point | p1, |
const geometry_msgs::Point | p2 | ||
) |
Definition at line 36 of file led_finder.cpp.
double robot_calibration::getDistance | ( | geometry_msgs::PointStamped & | p1, |
geometry_msgs::PointStamped & | p2 | ||
) |
Compute the distance in Euclidean space between two points. Note that points must be in the same frame.
Definition at line 80 of file data_functions.h.
std::vector<geometry_msgs::PointStamped> robot_calibration::getErrorPoints | ( | ChainModel * | chain1, |
ChainModel * | chain2, | ||
CalibrationOffsetParser * | offsets, | ||
robot_calibration_msgs::CalibrationData & | data | ||
) |
Returns a vector of points representing the differences between two projections using different chains but the same calibration offsets.
Definition at line 113 of file data_functions.h.
std::vector<double> robot_calibration::getErrors | ( | ChainModel * | chain1, |
ChainModel * | chain2, | ||
CalibrationOffsetParser * | offsets, | ||
robot_calibration_msgs::CalibrationData & | data | ||
) |
Returns the distance between each point in a given sample.
Definition at line 90 of file data_functions.h.
bool robot_calibration::hasSensor | ( | const robot_calibration_msgs::CalibrationData & | msg, |
const std::string & | sensor | ||
) |
Definition at line 54 of file optimizer.cpp.
bool robot_calibration::loadFeatureFinders | ( | ros::NodeHandle & | nh, |
FeatureFinderMap & | features | ||
) |
Load feature finders, based on param server config.
Definition at line 28 of file feature_finder.cpp.
double robot_calibration::positionFromMsg | ( | const std::string & | name, |
const sensor_msgs::JointState & | msg | ||
) |
Definition at line 26 of file models.cpp.
void robot_calibration::printComparePoints | ( | ChainModel * | chain1, |
ChainModel * | chain2, | ||
CalibrationOffsetParser * | before, | ||
CalibrationOffsetParser * | after, | ||
robot_calibration_msgs::CalibrationData & | data | ||
) |
Definition at line 173 of file data_functions.h.
void robot_calibration::printComparePointsInternal | ( | ChainModel * | chain1, |
ChainModel * | chain2, | ||
CalibrationOffsetParser * | offsets, | ||
robot_calibration_msgs::CalibrationData & | data | ||
) |
Definition at line 140 of file data_functions.h.
void robot_calibration::printSimpleDistanceError | ( | ChainModel * | chain1, |
ChainModel * | chain2, | ||
CalibrationOffsetParser * | before, | ||
CalibrationOffsetParser * | after, | ||
robot_calibration_msgs::CalibrationData & | data | ||
) |
Definition at line 130 of file data_functions.h.
KDL::Rotation robot_calibration::rotation_from_axis_magnitude | ( | const double | x, |
const double | y, | ||
const double | z | ||
) |
Converts our angle-axis-with-integrated-magnitude representation to a KDL::Rotation.
Definition at line 235 of file models.cpp.
sensor_msgs::CameraInfo robot_calibration::updateCameraInfo | ( | double | camera_fx, |
double | camera_fy, | ||
double | camera_cx, | ||
double | camera_cy, | ||
const sensor_msgs::CameraInfo & | info | ||
) | [inline] |
Update the camera calibration with the new offsets.
Definition at line 54 of file camera_info.h.
const unsigned robot_calibration::B = 2 |
Definition at line 34 of file led_finder.cpp.
const unsigned robot_calibration::G = 1 |
Definition at line 33 of file led_finder.cpp.
const unsigned robot_calibration::R = 0 |
Definition at line 32 of file led_finder.cpp.
const unsigned robot_calibration::X = 0 |
Definition at line 27 of file checkerboard_finder.cpp.
const unsigned robot_calibration::Y = 1 |
Definition at line 28 of file checkerboard_finder.cpp.
const unsigned robot_calibration::Z = 2 |
Definition at line 29 of file checkerboard_finder.cpp.