size_t CircleModel::getNumberOfFittedDataPoints(const list<Point>* data,
double maximumError) {
size_t numberOfFittedPoints = 0;
list<Point>::const_iterator pointIterator;
for (pointIterator = data->begin(); pointIterator != data->end();
++pointIterator) {
if (calculateError(&(*pointIterator)) <= maximumError) {
numberOfFittedPoints++;
}
}
return numberOfFittedPoints;
}
void RandomRestartLocalOptimization::getRandomState(LtoState& randomState) {
srand(time(0));
double trans_x = ((float) rand() / (float) (RAND_MAX)); //[0,1]
double trans_y = ((float) rand() / (float) (RAND_MAX)); //[0,1]
double trans_z = ((float) rand() / (float) (RAND_MAX)); //[0,1]
double rot_r = ((float) rand() / (float) (RAND_MAX)) * 2 * M_PI - M_PI; //[-Pi,Pi]
double rot_p = ((float) rand() / (float) (RAND_MAX)) * 2 * M_PI - M_PI; //[-Pi,Pi]
double rot_y = ((float) rand() / (float) (RAND_MAX)) * 2 * M_PI - M_PI; //[-Pi,Pi]
double offset_headyaw = ((float) rand() / (float) (RAND_MAX)); //[0,1]
double offset_headpitch = ((float) rand() / (float) (RAND_MAX)); //[0,1]
tf::Quaternion q;
q.setRPY(rot_r, rot_p, rot_y);
tf::Vector3 t(trans_x, trans_y, trans_z);
randomState.setCameraToHead(tf::Transform(q, t));
randomState.setHeadPitchOffset(offset_headpitch);
randomState.setHeadYawOffset(offset_headyaw);
randomState.error = calculateError(randomState);
}
void FunctionTree::crossover(FunctionTree* sibling){
checkTree();
sibling->checkTree();
int recombineIndexOne = rand()%size();
int recombineIndexTwo = rand()%sibling->size();
Function *recombineNodeOne = traverse(recombineIndexOne);
Function *recombineNodeTwo = sibling->traverse(recombineIndexTwo);
Function *parentNodeOne = recombineNodeOne->p;
Function *parentNodeTwo = recombineNodeTwo->p;
if (recombineIndexOne>0) {
if (recombineNodeOne->rChild) {
parentNodeOne->r = recombineNodeTwo;
} else {
parentNodeOne->l = recombineNodeTwo;
}
recombineNodeTwo->p = parentNodeOne;
} else {
rootNode = recombineNodeTwo;
recombineNodeTwo->p = NULL;
}
if (recombineIndexTwo>0) {
if (recombineNodeTwo->rChild) {
parentNodeTwo->r = recombineNodeOne;
} else {
parentNodeTwo->l = recombineNodeOne;
}
recombineNodeOne->p = parentNodeTwo;
} else {
sibling->rootNode = recombineNodeOne;
recombineNodeOne->p = NULL;
}
swap(recombineNodeOne->rChild, recombineNodeTwo->rChild);
trim();
writeFunctionString();
calculateError();
sibling->trim();
sibling->writeFunctionString();
sibling->calculateError();
}
void HillClimbingTransformOptimization::optimizeTransform(
CalibrationState& calibrationState) {
LtoState currentState, bestState;
currentState.setCameraToHead(this->getInitialCameraToHead());
currentState.error = calculateError(currentState);
bestState.error = INFINITY;
bool canImprove = true;
int numOfIterations = 0;
while (canImprove) {
if (numOfIterations++ % 100 == 0) {
//std::cout << currentState << std::endl;
}
std::vector<LtoState> neighbors = getNeighbors(currentState);
LtoState bestNeighbor;
bestNeighbor.error = INFINITY;
// find the best neighbor
for (int i = 0; i < neighbors.size(); i++) {
LtoState currentNeighbor = neighbors[i];
if (currentNeighbor.isBetterThan(bestNeighbor)) {
bestNeighbor = currentNeighbor;
}
}
// check whether the current best neighbor improves
if (bestNeighbor.isBetterThan(currentState)) {
currentState = bestNeighbor;
stepwidth = 0.1;
} else if (!decreaseStepwidth()) {
canImprove = false;
}
}
calibrationState.setCameraToHead(currentState.getCameraToHead());
calibrationState.setHeadYawOffset(currentState.getHeadYawOffset());
calibrationState.setHeadPitchOffset(currentState.getHeadPitchOffset());
}
void FunctionTree::mutate(){
int mutateIndex = rand()%size();
Function *mutateNode = traverse(mutateIndex);
Function *parentNode;
parentNode = mutateNode->p;
bool rightChild = mutateNode->rChild;
delete mutateNode;
mutateNode = new Function;
if (parentNode!=NULL) {
mutateNode->p = parentNode;
mutateNode->rChild=rightChild;
if (rightChild) {
parentNode->r = mutateNode;
} else {
parentNode->l = mutateNode;
}
}
if ((randomDouble()) > 0.5) generateFull(mutateNode, maxTreeDepth-mutateNode->getDepth());
else generateGrow(mutateNode, maxTreeDepth-mutateNode->getDepth());
if(mutateIndex==0) rootNode = mutateNode;
trim();
writeFunctionString();
calculateError();
}
int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points,
int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations)
{
// A straightforward one-level implementation of Lucas-Kanade.
// For all points:
// (1) determine the subpixel neighborhood in the old image
// (2) get the x- and y- gradients
// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
// (4) iterate over taking steps in the image to minimize the error:
// [a] get the subpixel neighborhood in the new image
// [b] determine the image difference between the two neighborhoods
// [c] calculate the 'b'-vector
// [d] calculate the additional flow step and possibly terminate the iteration
int p, subpixel_factor, x, y, it, step_threshold, step_x, step_y, v_x, v_y, Det;
int b_x, b_y, patch_size, padded_patch_size, error;
unsigned int ix1, ix2;
int *I_padded_neighborhood; int *I_neighborhood; int *J_neighborhood;
int *DX; int *DY; int *ImDiff; int *IDDX; int *IDDY;
int G[4];
int error_threshold;
// set the image width and height
IMG_WIDTH = imW;
IMG_HEIGHT = imH;
// spatial resolution of flow is 1 / subpixel_factor
subpixel_factor = 10;
// determine patch sizes and initialize neighborhoods
patch_size = (2 * half_window_size + 1);
error_threshold = (25 * 25) * (patch_size * patch_size);
padded_patch_size = (2 * half_window_size + 3);
I_padded_neighborhood = (int *) malloc(padded_patch_size * padded_patch_size * sizeof(int));
I_neighborhood = (int *) malloc(patch_size * patch_size * sizeof(int));
J_neighborhood = (int *) malloc(patch_size * patch_size * sizeof(int));
if (I_padded_neighborhood == 0 || I_neighborhood == 0 || J_neighborhood == 0) {
return NO_MEMORY;
}
DX = (int *) malloc(patch_size * patch_size * sizeof(int));
DY = (int *) malloc(patch_size * patch_size * sizeof(int));
IDDX = (int *) malloc(patch_size * patch_size * sizeof(int));
IDDY = (int *) malloc(patch_size * patch_size * sizeof(int));
ImDiff = (int *) malloc(patch_size * patch_size * sizeof(int));
if (DX == 0 || DY == 0 || ImDiff == 0 || IDDX == 0 || IDDY == 0) {
return NO_MEMORY;
}
for (p = 0; p < n_found_points; p++) {
// status: point is not yet lost:
status[p] = 1;
// We want to be able to take steps in the image of 1 / subpixel_factor:
p_x[p] *= subpixel_factor;
p_y[p] *= subpixel_factor;
// if the pixel is outside the ROI in the image, do not track it:
if (!(p_x[p] > ((half_window_size + 1) * subpixel_factor) && p_x[p] < (IMG_WIDTH - half_window_size) * subpixel_factor
&& p_y[p] > ((half_window_size + 1) * subpixel_factor) && p_y[p] < (IMG_HEIGHT - half_window_size)*subpixel_factor)) {
status[p] = 0;
}
// (1) determine the subpixel neighborhood in the old image
// we determine a padded neighborhood with the aim of subsequent gradient processing:
getSubPixel_gray(I_padded_neighborhood, old_image_buf, p_x[p], p_y[p], half_window_size + 1, subpixel_factor);
// Also get the original-sized neighborhood
for (x = 1; x < padded_patch_size - 1; x++) {
for (y = 1; y < padded_patch_size - 1; y++) {
ix1 = (y * padded_patch_size + x);
ix2 = ((y - 1) * patch_size + (x - 1));
I_neighborhood[ix2] = I_padded_neighborhood[ix1];
}
}
// (2) get the x- and y- gradients
getGradientPatch(I_padded_neighborhood, DX, DY, half_window_size);
// (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
error = calculateG(G, DX, DY, half_window_size);
if (error == NO_MEMORY) { return NO_MEMORY; }
for (it = 0; it < 4; it++) {
G[it] /= 255; // to keep values in range
}
// calculate G's determinant:
Det = G[0] * G[3] - G[1] * G[2];
Det = Det / subpixel_factor; // so that the steps will be expressed in subpixel units
if (Det < 1) {
status[p] = 0;
}
// (4) iterate over taking steps in the image to minimize the error:
it = 0;
step_threshold = 2; // 0.2 as smallest step (L1)
v_x = 0;
v_y = 0;
step_x = step_threshold + 1;
step_y = step_threshold + 1;
while (status[p] == 1 && it < max_iterations && (abs(step_x) >= step_threshold || abs(step_y) >= step_threshold)) {
// if the pixel goes outside the ROI in the image, stop tracking:
if (!(p_x[p] + v_x > ((half_window_size + 1) * subpixel_factor)
&& p_x[p] + v_x < ((int)IMG_WIDTH - half_window_size) * subpixel_factor
&& p_y[p] + v_y > ((half_window_size + 1) * subpixel_factor)
&& p_y[p] + v_y < ((int)IMG_HEIGHT - half_window_size)*subpixel_factor)) {
status[p] = 0;
break;
}
// [a] get the subpixel neighborhood in the new image
// clear J:
for (x = 0; x < patch_size; x++) {
for (y = 0; y < patch_size; y++) {
ix2 = (y * patch_size + x);
J_neighborhood[ix2] = 0;
}
}
getSubPixel_gray(J_neighborhood, new_image_buf, p_x[p] + v_x, p_y[p] + v_y, half_window_size, subpixel_factor);
// [b] determine the image difference between the two neighborhoods
getImageDifference(I_neighborhood, J_neighborhood, ImDiff, patch_size, patch_size);
error = calculateError(ImDiff, patch_size, patch_size) / 255;
if (error > error_threshold && it > max_iterations / 2) {
status[p] = 0;
break;
}
multiplyImages(ImDiff, DX, IDDX, patch_size, patch_size);
b_x = getSumPatch(IDDX, patch_size) / 255;
b_y = getSumPatch(IDDY, patch_size) / 255;
//printf("b_x = %d; b_y = %d;\n\r", b_x, b_y);
// [d] calculate the additional flow step and possibly terminate the iteration
step_x = (G[3] * b_x - G[1] * b_y) / Det;
step_y = (G[0] * b_y - G[2] * b_x) / Det;
v_x += step_x;
v_y += step_y; // - (?) since the origin in the image is in the top left of the image, with y positive pointing down
// next iteration
it++;
} // iteration to find the right window in the new image
new_x[p] = (p_x[p] + v_x) / subpixel_factor;
new_y[p] = (p_y[p] + v_y) / subpixel_factor;
p_x[p] /= subpixel_factor;
p_y[p] /= subpixel_factor;
}
// free all allocated variables:
free((int *) I_padded_neighborhood);
free((int *) I_neighborhood);
free((int *) J_neighborhood);
free((int *) DX);
free((int *) DY);
free((int *) ImDiff);
free((int *) IDDX);
free((int *) IDDY);
// no errors:
return OK;
}
bool eval(TinyVector<FLT,tet_mesh::ND> x, double t, Array<double,1> &val) const {
double q = q0;
double theta = theta0;
double dtheta = -1.0e-4;
double dq = 1.0e-4;
double jac[2][2], deti, delta[2];
bool conservative = true;
double xyz[3];
xyz[0] = scale*x(0) +shift[0];
xyz[1] = scale*x(1) +shift[1];
if (fabs(xyz[1]) < 1.0e-8)
theta = M_PI/2.0;
double error[2] = {1.0, 1.0};
int niter = 0;
while (fabs(error[0])+fabs(error[1]) > 1.0e-15 && niter++ < 30) {
calculateError(xyz, error, q, theta);
calculateError(xyz, delta, q+dq, theta);
jac[0][0] = (delta[0]-error[0])/dq;
jac[1][0] = (delta[1]-error[1])/dq;
calculateError(xyz, delta, q, theta+dtheta);
jac[0][1] = (delta[0]-error[0])/dtheta;
jac[1][1] = (delta[1]-error[1])/dtheta;
deti = 1./(jac[0][0]*jac[1][1] -jac[0][1]*jac[1][0]);
if (fabs(xyz[1]) > 1.0e-6) {
delta[0] = deti*(error[0]*jac[1][1] -error[1]*jac[0][1]);
delta[1] = deti*(error[1]*jac[0][0] -error[0]*jac[1][0]);
q -= delta[0];
theta -= delta[1];
}
else {
q -= error[0]/jac[0][0];
theta = M_PI/2.;
}
if (fabs(theta) > M_PI/2.) {
theta = M_PI/2. -(theta-M_PI/2.);
}
}
if (niter > 9999 || !(q >= 0.0)) {
std::cout << "RINGLEB NOT_CONVERGED: " << xyz[0] << ' ' << xyz[1] << niter << ' ' << q << ' ' << theta << std::endl;
return false;
}
double c = sqrt(1. - (gam-1.)/2.*q*q);
double r = pow(c,(2./(gam-1.)));
if (conservative) {
val(0) = r;
val(1) = r*q*cos(theta)*sin(theta1);
val(2) = r*q*sin(theta)*sin(theta1);
val(3) = r*q*cos(theta1);
val(4) = r*(c*c/(gam*(gam-1.)) + 0.5*q*q);
}
else {
val(0) = r*c*c/gam;
val(1) = q*cos(theta)*sin(theta1);
val(2) = q*sin(theta)*sin(theta1);
val(3) = q*cos(theta1);
val(4) = val(0)/r;
}
return true;
}
FunctionTree::FunctionTree(FunctionTree const &original){
rootNode = new Function(*original.rootNode);
writeFunctionString();
calculateError();
}
double
VbitmapComputePSNR(Vbitmap *vbitmap, Vbitmap *reference)
{
int width;
int height;
unsigned char* v1buffer;
unsigned char* v2buffer;
int bpp1;
int bpp2;
int v1pitch;
int v2pitch;
double error = 0.0;
double psnr = -1.0;
if (vbitmap == NULL || reference == NULL ||
VbitmapWidth(vbitmap) != VbitmapWidth(reference) ||
VbitmapHeight(vbitmap) != VbitmapHeight(reference)) {
return psnr;
}
if (vbitmap == reference) {
return VBITMAP_MAX_PSNR;
}
bpp1 = VbitmapBpp(vbitmap);
bpp2 = VbitmapBpp(reference);
if (VbitmapLock(vbitmap) == YMAGINE_OK) {
if (VbitmapLock(reference) == YMAGINE_OK) {
v1buffer = VbitmapBuffer(vbitmap);
v2buffer = VbitmapBuffer(reference);
if (v1buffer != NULL && v2buffer != NULL) {
width = VbitmapWidth(vbitmap);
height = VbitmapHeight(vbitmap);
v1pitch = VbitmapPitch(vbitmap);
v2pitch = VbitmapPitch(reference);
if (bpp1 == 1 && bpp2 == 1) {
error = calculateError(width, height, v1buffer, v2buffer, 1, 1, v1pitch, v2pitch, 1);
} else if (bpp1 == 3 && bpp2 == 3) {
error = calculateError(width, height, v1buffer, v2buffer, 3, 3, v1pitch, v2pitch, 3);
} else if (bpp1 == 4 && bpp2 == 4) {
error = calculateError(width, height, v1buffer, v2buffer, 4, 4, v1pitch, v2pitch, 4);
} else {
int bpp = MIN(bpp1, bpp2);
error = calculateError(width, height, v1buffer, v2buffer, bpp1, bpp2, v1pitch, v2pitch, bpp);
}
}
VbitmapUnlock(reference);
}
VbitmapUnlock(vbitmap);
}
if (error > 1.0e-10) {
psnr = (10.0 / LOG10) * log((255.0 * 255.0) / error);
if (psnr > VBITMAP_MAX_PSNR) {
psnr = VBITMAP_MAX_PSNR;
}
} else {
psnr = VBITMAP_MAX_PSNR;
}
return psnr;
}
intensityCloud::Ptr ICP::getTransformation(avora_msgs::StampedIntensityCloudPtr P0, MLSM *X, Vector3d eV, Vector3d eT, Matrix4f eR, Vector3d *Tv, Vector3d *T, Matrix4f *R){
double error = DBL_MAX;
unsigned int iterations = 0;
std::vector<BlockInfo> Y;
intensityCloud cloud, cloud0;
pcl::fromROSMsg(P0->cloud,cloud);
pcl::fromROSMsg(P0->cloud,cloud0);
std::vector<Vector3d> transforms(cloud.size());
intensityCloud::Ptr P = boost::make_shared<intensityCloud>(cloud);
// Initial transform
ROS_INFO("Applying initial transformation");
Vector3d accumulatedT;
Vector3d accumulatedTv;
Matrix4f accumulatedR = Matrix<float, 4, 4>::Identity();
accumulatedT[0] = 0;
accumulatedT[1] = 0;
accumulatedT[2] = 0;
accumulatedTv[0] = 0;//eV[0];
accumulatedTv[1] = 0;//eV[1];
accumulatedTv[2] = 0;//eV[2];
(*Tv)[0] = 0;//eV[0];
(*Tv)[1] = 0;//eV[1];
(*Tv)[2] = 0;//eV[2];
//std::vector<Vector3d> transforms = applyTransformation(P, P0->timeStamps, eR, eV, eT);
ROS_INFO("Initial transformation applied");
//intensityCloud sum;
sensor_msgs::PointCloud2 debugCloud;
//sum = cloud0 + *P;
//pcl::toROSMsg(*P,debugCloud);
//debugCloud.header.frame_id = "map";
//debugPublisher_->publish(debugCloud);
//sleep(5);
while ((iterations < maxIterations_) && (error > errorThreshold_)){
(*R) = Matrix<float, 4, 4>::Identity();
// Calculate closest points
ROS_INFO("Get closest points");
Y = closestPoints(P,X, P0->timeStamps,eV);
// Calculate transformation
ROS_INFO("Get transformation");
error = registration(P,&Y,P0->timeStamps,&transforms, T, R,eV);
// Iterate through P applying the correct transformation depending on Tv and the stamp
ROS_INFO("Applying transformation");
//transforms = applyTransformation(P,P0->timeStamps, *R, *Tv, *T);
applyTransformation(P, *R, transforms);
// Calculate error
ROS_INFO("Calculate error");
error = calculateError(P, &Y);
//ROS_INFO("%d error = %f",iterations, error);
// Iterate?
iterations++;
accumulatedT += *T;
//accumulatedT += transforms.back();
//accumulatedTv += *Tv;
//ROS_INFO("\n%d \tAcc Vel x = %f y = %f z = %f error = %f",iterations,accumulatedTv[0], accumulatedTv[1], accumulatedTv[2], error);
accumulatedR = accumulatedR * (*R);
ROS_INFO("\n%d \tTrl x = %f y = %f z = %f error = %f \n\tAcc x = %f y = %f z = %f",iterations,(*T)[0], (*T)[1], (*T)[2], error,
accumulatedT[0], accumulatedT[1], accumulatedT[2]);
//ROS_INFO("\n%d \tRotation \n%f %f %f\n%f %f %f\n%f %f %f",iterations,accumulatedR(0,0),accumulatedR(0,1),accumulatedR(0,2),
// accumulatedR(1,0),accumulatedR(1,1),accumulatedR(1,2),
// accumulatedR(2,0),accumulatedR(2,1),accumulatedR(2,2));
//pcl::toROSMsg(*P,debugCloud);
//debugCloud.header.frame_id = "map";
//debugPublisher_->publish(debugCloud);
//sleep(1);
}
pcl::toROSMsg(*P,debugCloud);
debugCloud.header.frame_id = "map";
debugPublisher_->publish(debugCloud);
//(*T)[0] = //(*Tv)[0] * (P0->timeStamps.back() - P0->timeStamps.front());
//(*T)[1] = //(*Tv)[1] * (P0->timeStamps.back() - P0->timeStamps.front());
//(*T)[2] = //(*Tv)[2] * (P0->timeStamps.back() - P0->timeStamps.front());
(*T) = accumulatedT;
(*R) = accumulatedR;
ROS_INFO("Scan time: %f",fabs(P0->timeStamps.back()) - fabs(P0->timeStamps.front()));
ROS_INFO("\n%d \tMoved x = %f y = %f z = %f error = %f ",iterations,(*T)[0], (*T)[1], (*T)[2], error);
//ROS_INFO("\n%d \tRotation \n%f %f %f\n%f %f %f\n%f %f %f",iterations,accumulatedR(0,0),accumulatedR(0,1),accumulatedR(0,2),
// accumulatedR(1,0),accumulatedR(1,1),accumulatedR(1,2),
// accumulatedR(2,0),accumulatedR(2,1),accumulatedR(2,2));
//(*T) = accumulatedT;
return P;
//return error < errorThreshold_;
}
void SimulatedAnnealingTransformOptimization::optimizeTransform(
CalibrationState& calibrationState) {
srand(time(0));
int i = 0;
LtoState initialState;
initialState.setCameraToHead(this->getInitialCameraToHead());
initialState.error = calculateError(initialState);
/* temperature */
double temperature = startTemperature; //startTemperature;
/* iterations */
int iterations = maxIterations;
/* current state */
LtoState currentState = initialState;
/* successor state */
LtoState successorState = currentState;
/* best state */
LtoState bestState = currentState;
double difference = 0;
while (iterations-- > 0 && temperature > 0) {
//cout << "iteration " << iterations << " temperature " << temperature
// << " currentState " << currentState << endl;
// store best state found so far
if (currentState.isBetterThan(bestState)) {
bestState = currentState;
}
std::vector<LtoState> neighbors = getNeighbors(currentState);
i = rand() % neighbors.size();
successorState = neighbors[i];
difference = currentState.error - successorState.error;
if (iterations % 10 == 0) {
cout << "temperature " << temperature << endl;
cout << successorState;
cout << "currentState.error " << currentState.error << endl;
cout << "successorState.error " << successorState.error << endl;
cout << "exp(difference / temperature) "
<< exp(difference / temperature) << endl;
}
if (difference > 0) {
// always improve
currentState = successorState;
//cout << "--> improves" << endl;
} else if (exp(difference / temperature)
> ((float) rand() / (float) (RAND_MAX))) {
// take a worse state only depending on the temperature
currentState = successorState;
//cout << "--> taking anyway" << endl;
} else {
//cout << "--> not taking" << endl;
}
temperature -= startTemperature / maxIterations;
// temperature = 30.35044905 - 1.841778626 * log(1000000 -
// maxIterations);
}
calibrationState.setCameraToHead(bestState.getCameraToHead());
calibrationState.setHeadYawOffset(bestState.getHeadYawOffset());
calibrationState.setHeadPitchOffset(bestState.getHeadPitchOffset());
}
std::vector<LtoState> LocalTransformOptimization::getNeighbors(
LtoState& current) {
std::vector<LtoState> neighbours;
std::vector<tf::Transform> transforms;
tf::Transform currentTransform = current.getCameraToHead();
// get rotation
double roll, pitch, yaw;
tf::Quaternion rotation = currentTransform.getRotation();
tf::Matrix3x3(rotation).getRPY(roll, pitch, yaw, 1);
// get translation
double x, y, z;
tf::Vector3 translation = currentTransform.getOrigin();
x = translation.getX();
y = translation.getY();
z = translation.getZ();
// create neighbor states
transforms.push_back(
tf::Transform(
tf::createQuaternionFromRPY(roll + stepwidth, pitch, yaw),
tf::Vector3(x, y, z)));
transforms.push_back(
tf::Transform(
tf::createQuaternionFromRPY(roll, pitch + stepwidth, yaw),
tf::Vector3(x, y, z)));
transforms.push_back(
tf::Transform(
tf::createQuaternionFromRPY(roll, pitch, yaw + stepwidth),
tf::Vector3(x, y, z)));
transforms.push_back(
tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
tf::Vector3(x + stepwidth, y, z)));
transforms.push_back(
tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
tf::Vector3(x, y + stepwidth, z)));
transforms.push_back(
tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
tf::Vector3(x, y, z + stepwidth)));
transforms.push_back(
tf::Transform(
tf::createQuaternionFromRPY(roll - stepwidth, pitch, yaw),
tf::Vector3(x, y, z)));
transforms.push_back(
tf::Transform(
tf::createQuaternionFromRPY(roll, pitch - stepwidth, yaw),
tf::Vector3(x, y, z)));
transforms.push_back(
tf::Transform(
tf::createQuaternionFromRPY(roll, pitch, yaw - stepwidth),
tf::Vector3(x, y, z)));
transforms.push_back(
tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
tf::Vector3(x - stepwidth, y, z)));
transforms.push_back(
tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
tf::Vector3(x, y - stepwidth, z)));
transforms.push_back(
tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
tf::Vector3(x, y, z - stepwidth)));
// calculate errors
for (int i = 0; i < transforms.size(); i++) {
LtoState newState;
newState.setCameraToHead(transforms[i]);
newState.setHeadPitchOffset(current.getHeadPitchOffset());
newState.setHeadYawOffset(current.getHeadYawOffset());
double error = calculateError(newState);
newState.error = error;
neighbours.push_back(newState);
}
if (parameter.isCalibrateJointOffsets()) {
{
LtoState newState;
newState.setCameraToHead(currentTransform);
newState.setHeadPitchOffset(
current.getHeadPitchOffset() + stepwidth);
newState.setHeadYawOffset(current.getHeadYawOffset());
double error = calculateError(newState);
newState.error = error;
neighbours.push_back(newState);
}
{
LtoState newState;
newState.setCameraToHead(currentTransform);
newState.setHeadPitchOffset(
current.getHeadPitchOffset() - stepwidth);
newState.setHeadYawOffset(current.getHeadYawOffset());
double error = calculateError(newState);
newState.error = error;
neighbours.push_back(newState);
}
{
LtoState newState;
newState.setCameraToHead(currentTransform);
newState.setHeadPitchOffset(current.getHeadPitchOffset());
newState.setHeadYawOffset(current.getHeadYawOffset() + stepwidth);
double error = calculateError(newState);
newState.error = error;
neighbours.push_back(newState);
}
{
LtoState newState;
newState.setCameraToHead(currentTransform);
newState.setHeadPitchOffset(current.getHeadPitchOffset());
newState.setHeadYawOffset(current.getHeadYawOffset() - stepwidth);
double error = calculateError(newState);
newState.error = error;
neighbours.push_back(newState);
}
}
return neighbours;
}
