/*******************************+++*******************************/
unsigned MinAnyX(real (*ObjFunc)(real *x, size_t nDims),
real AbsTol, real RelTol, unsigned MaxFuncs,
const Matrix *XReg, size_t nDims, int MinAlg, real *x,
real *Obj)
/*****************************************************************/
/* Purpose: Optimize any objective function of x, where x may */
/* include fixed, discrete, or continuous */
/* variables. */
/* */
/* Args: ObjFunc The objective function. */
/* AbsTol Absolute tolerance on function value */
/* for convergence. */
/* RelTol Relative tolerance on function value */
/* for convergence. */
/* MaxFuncs Maximum function evaluations. */
/* XReg X-variable feasibility region. */
/* nDims Number of dimensions. */
/* MinAlg The minimizer called for unconstrained */
/* minimization. */
/* x Input: Starting point (including any */
/* fixed variables); */
/* Output: "Optimal" point. */
/* Obj Input: Objective at x; */
/* Output: "Optimal" objective. */
/* */
/* Returns: Total number of function evaluations. */
/* */
/* 96.01.18: CodeBug parameters changed and CodeBug replaced */
/* by CodeCheck. */
/* 96.01.18: CodeBug parameters changed. */
/* 96.03.07: Extrapolation at end of each iteration. */
/* 96.03.08: MinConverged replaced by ApproxEq. */
/* Extrapolation removed. */
/* 96.03.09: Extrapolation at end of each iteration. */
/* */
/* Version: 1996.03.09 */
/*****************************************************************/
{
real ObjOld;
real *ContMax, *ContMin, *xCont, *xOld;
real *xExtCopy;
real (*ObjFuncExtCopy)(real *x, size_t nDims);
size_t i, ThisGroup;
size_t *ContDistrib, **GroupIndex, *Group;
size_t *GroupSize, *UngroupedIndex;
size_t *IndexContCopy, nDimsExtCopy;
size_t j, nContVars, nGroups, nUngroupedVars;
unsigned nEvals, NumOpts;
/* Save statics to local variables, */
/* to enable recursive calling. */
ObjFuncExtCopy = ObjFuncExt;
xExtCopy = xExt;
IndexContCopy = IndexCont;
nDimsExtCopy = nDimsExt;
CodeCheck(RegNumVars(XReg) == nDims);
ContDistrib = AllocSize_t(nDims, NULL);
IndexCont = AllocSize_t(nDims, NULL);
ContMax = AllocReal(nDims, NULL);
ContMin = AllocReal(nDims, NULL);
xCont = AllocReal(nDims, NULL);
xOld = AllocReal(nDims, NULL);
UngroupedIndex = AllocSize_t(nDims, NULL);
GroupIndex = AllocPtrSize_t(nDims, NULL);
Group = AllocSize_t(nDims, NULL);
GroupSize = AllocSize_t(nDims, NULL);
/* Number of continuous variables. */
nContVars = 0;
/* Number of GRID or ungrouped DISCRETE variables. */
nUngroupedVars = 0;
/* Number of grouped DISCRETE variables. */
nGroups = 0;
/* Count the continuous variables, etc. */
for (j = 0; j < nDims; j++)
switch (RegSupport(XReg, j))
{
case FIXED:
break;
case CONTINUOUS:
IndexCont[nContVars] = j;
ContMin[nContVars] = RegMin(XReg, j);
ContMax[nContVars] = RegMax(XReg, j);
ContDistrib[nContVars++]
= RegDistrib(XReg, j);
break;
case GRID:
UngroupedIndex[nUngroupedVars++] = j;
break;
case DISCRETE:
ThisGroup = RegCandGroup(XReg, j);
if (ThisGroup == 0)
/* Ungrouped. */
UngroupedIndex[nUngroupedVars++] = j;
else
{
for (i = 0; i < nGroups; i++)
if (ThisGroup == Group[i])
break;
if (i == nGroups)
{
/* New candidate group. */
Group[i] = ThisGroup;
GroupSize[i] = 1;
GroupIndex[i] = AllocSize_t(nDims, NULL);
GroupIndex[i][0] = j;
nGroups++;
}
else
/* Existing candidate group. */
GroupIndex[i][GroupSize[i]++] = j;
}
break;
default:
CodeBug("Illegal support");
}
/* External equivalents. */
ObjFuncExt = ObjFunc;
nDimsExt = nDims;
xExt = x;
/* Iterate until converged. */
nEvals = 0;
do
{
/* Used in test for convergence. */
ObjOld = *Obj;
NumOpts = 0;
VecCopy(x, nDims, xOld);
if (nContVars > 0)
{
/* Load continuous x's into xCont. */
VecCopyIndex(nContVars, IndexCont, x, NULL, xCont);
nEvals += MinCont(ObjCont, AbsTol, RelTol, MaxFuncs,
ContMin, ContMax, ContDistrib, nContVars,
MinAlg, xCont, Obj);
NumOpts++;
/* Put best continuous levels back in x. */
VecCopyIndex(nContVars, NULL, xCont, IndexCont, x);
}
/* Ungrouped variables. */
for (j = 0; j < nUngroupedVars; j++)
{
/* Optimize ungrouped-variable j. */
nEvals += MinDisc(1, &UngroupedIndex[j], XReg, x,
Obj);
NumOpts++;
}
/* Grouped variables. */
for (j = 0; j < nGroups; j++)
{
/* Optimize group j. */
nEvals += MinDisc(GroupSize[j], GroupIndex[j],
XReg, x, Obj);
NumOpts++;
}
/* Try extrapolating. */
nEvals += MinExtrap(ObjFunc, XReg, nDims, xOld, x, Obj);
} while (NumOpts > 1 &&
!ApproxEq(ObjOld, *Obj, AbsTol, RelTol));
AllocFree(ContDistrib);
AllocFree(IndexCont);
AllocFree(ContMax);
AllocFree(ContMin);
AllocFree(xCont);
AllocFree(xOld);
AllocFree(UngroupedIndex);
for (j = 0; j < nGroups; j++)
AllocFree(GroupIndex[j]);
AllocFree(GroupIndex);
AllocFree(Group);
AllocFree(GroupSize);
/* Restore statics. */
ObjFuncExt = ObjFuncExtCopy;
xExt = xExtCopy;
IndexCont = IndexContCopy;
nDimsExt = nDimsExtCopy;
return nEvals;
}
bool Depth::processCameraInfo(
const sensor_msgs::CameraInfoConstPtr& first_camera_info,
const sensor_msgs::CameraInfoConstPtr& second_camera_info, double* baseline,
double* focal_length, bool* first_is_left, int* cx, int* cy) {
if (first_camera_info->height != second_camera_info->height) {
ROS_ERROR("Image heights do not match");
return false;
}
if (first_camera_info->width != second_camera_info->width) {
ROS_ERROR("Image widths do not match");
return false;
}
for (double d : first_camera_info->D) {
if (!ApproxEq(d, 0)) {
ROS_ERROR("First image has non-zero distortion");
return false;
}
}
for (double d : second_camera_info->D) {
if (!ApproxEq(d, 0)) {
ROS_ERROR("Second image has non-zero distortion");
return false;
}
}
for (size_t i = 0; i < 12; ++i) {
if ((i != 3) &&
!ApproxEq(first_camera_info->P[i], second_camera_info->P[i])) {
ROS_ERROR("Image P matrices must match (excluding x offset)");
return false;
}
}
if (!ApproxEq(first_camera_info->P[1], 0) ||
!ApproxEq(first_camera_info->P[4], 0)) {
ROS_ERROR("Image P matrix contains skew");
return false;
}
if (!ApproxEq(first_camera_info->P[0], first_camera_info->P[5])) {
ROS_ERROR("Image P matrix has different values for Fx and Fy");
return false;
}
if (first_camera_info->P[0] <= 0) {
ROS_ERROR("Focal length must be greater than 0");
return false;
}
// downgraded to warning so that the color images of the KITTI dataset can be
// processed
if (!ApproxEq(first_camera_info->P[8], 0) ||
!ApproxEq(first_camera_info->P[9], 0) ||
!ApproxEq(first_camera_info->P[10], 1) ||
!ApproxEq(first_camera_info->P[11], 0)) {
ROS_WARN_ONCE(
"Image P matrix does not end in [0,0,1,0], these values will be "
"ignored");
}
// again downgraded to warning because KITTI has ugly matrices
if (!ApproxEq(first_camera_info->P[7], 0)) {
ROS_WARN_ONCE("P contains Y offset, this value will be ignored");
}
*focal_length = first_camera_info->P[0];
*baseline = (second_camera_info->P[3] - first_camera_info->P[3]) /
first_camera_info->P[0];
if (*baseline > 0) {
*first_is_left = false;
} else {
*first_is_left = true;
*baseline *= -1;
}
*cx = first_camera_info->P[2];
*cy = first_camera_info->P[6];
return true;
}
