Spectrum PerspectiveSensor::sampleRay(Ray3f& ray, const Point2f& samplePosition, const Point2f& apertureSample) const
{
Point3f nearPoint = m_sampleToCamera * Point3f(
samplePosition.x() * m_invImageSize.x(),
samplePosition.y() * m_invImageSize.y(), 0.f);
Vector3f dir = nearPoint.normalized();
float invZ = 1.f / dir.z();
ray.o = m_cameraToWorld * Point3f(0.f, 0.f, 0.f);
ray.d = m_cameraToWorld * dir;
ray.minT = m_nearClip * invZ;
ray.maxT = m_farClip * invZ;
ray.update();
return Spectrum(1.f);
}
void Arm::solve(Point3f goal_point, int life_count) {
// prev and curr are for use of halving
// last is making sure the iteration gets a better solution than the last iteration,
// otherwise revert changes
float prev_err, curr_err, last_err = 9999;
Point3f current_point;
int max_iterations = 200;
int count = 0;
float err_margin = 0.01;
goal_point -= base;
if (goal_point.norm() > get_max_length()) {
goal_point = goal_point.normalized() * get_max_length();
}
current_point = calculate_end_effector();
// save the first err
prev_err = (goal_point - current_point).norm();
curr_err = prev_err;
last_err = curr_err;
// while the current point is close enough, stop iterating
while (curr_err > err_margin) {
// calculate the difference between the goal_point and current_point
Vector3f dP = goal_point - current_point;
// create the jacovian
int segment_size = segments.size();
// build the transpose matrix (easier for eigen matrix construction)
MatrixXf jac_t(3*segment_size, 3);
for(int i=0; i<3*segment_size; i+=3) {
Matrix<float, 1, 3> row_theta = compute_jacovian_segment(i/3, goal_point, segments[i/3]->get_right());
Matrix<float, 1, 3> row_phi = compute_jacovian_segment(i/3, goal_point, segments[i/3]->get_up());
Matrix<float, 1, 3> row_z = compute_jacovian_segment(i/3, goal_point, segments[i/3]->get_z());
jac_t(i, 0) = row_theta(0, 0);
jac_t(i, 1) = row_theta(0, 1);
jac_t(i, 2) = row_theta(0, 2);
jac_t(i+1, 0) = row_phi(0, 0);
jac_t(i+1, 1) = row_phi(0, 1);
jac_t(i+1, 2) = row_phi(0, 2);
jac_t(i+2, 0) = row_z(0, 0);
jac_t(i+2, 1) = row_z(0, 1);
jac_t(i+2, 2) = row_z(0, 2);
}
// compute the final jacovian
MatrixXf jac(3, 3*segment_size);
jac = jac_t.transpose();
Matrix<float, Dynamic, Dynamic> pseudo_ijac;
MatrixXf pinv_jac(3*segment_size, 3);
pinv_jac = pseudoInverse(jac);
Matrix<float, Dynamic, 1> changes = pinv_jac * dP;
cout << "changes: " << changes << endl;
for(int i=0; i<3*segment_size; i+=3) {
// save the current transformation on the segments
segments[i/3]->save_transformation();
// apply the change to the theta angle
segments[i/3]->apply_angle_change(changes[i], segments[i/3]->get_right());
// apply the change to the phi angle
segments[i/3]->apply_angle_change(changes[i+1], segments[i/3]->get_up());
// apply the change to the z angle
segments[i/3]->apply_angle_change(changes[i+2], segments[i/3]->get_z());
}
// compute current_point after making changes
current_point = calculate_end_effector();
//cout << "current_point: " << vectorString(current_point) << endl;
//cout << "goal_point: " << vectorString(goal_point) << endl;
prev_err = curr_err;
curr_err = (goal_point - current_point).norm();
int halving_count = 0;
cout << "curr err: " << curr_err << " || prev err: " << prev_err << " || last err: " << last_err << endl;
// make sure we aren't iterating past the solution
while (curr_err > last_err) {
// undo changes
for(int i=0; i<segment_size; i++) {
// unapply the change to the saved angle
segments[i]->load_transformation();
}
current_point = calculate_end_effector();
changes *= 0.5;
// reapply halved changes
for(int i=0; i<3*segment_size; i+=3) {
// save the current transformation on the segments
segments[i/3]->save_transformation();
// apply the change to the theta angle
segments[i/3]->apply_angle_change(changes[i], segments[i/3]->get_right());
// apply the change to the phi angle
segments[i/3]->apply_angle_change(changes[i+1], segments[i/3]->get_up());
// apply the change to the z angle
segments[i/3]->apply_angle_change(changes[i+2], segments[i/3]->get_z());
}
// compute the end_effector and measure error
current_point = calculate_end_effector();
prev_err = curr_err;
curr_err = (goal_point - current_point).norm();
cout << "|half| curr err: " << curr_err << " || prev err: " << prev_err << endl;
halving_count++;
if (halving_count > 100)
break;
}
if (curr_err > last_err) {
// undo changes
for(int i=0; i<segment_size; i++) {
// unapply the change to the saved angle
segments[i]->load_last_transformation();
}
current_point = calculate_end_effector();
curr_err = (goal_point - current_point).norm();
cout << "curr iteration not better than last, reverting" << endl;
cout << "curr err: " << curr_err << " || last err: " << last_err << endl;
break;
}
for(int i=0; i<segment_size; i++) {
// unapply the change to the saved angle
segments[i]->save_last_transformation();
}
cout << "curr err: " << curr_err << " || last err: " << last_err << endl;
last_err = curr_err;
cout << "last_err is now : " << last_err << endl;
// make sure we don't infinite loop
count++;
if (count > max_iterations) {
break;
}
}
/*
// if we haven't gotten to a nice solution
if (curr_err > err_margin) {
// kill off infinitely recursive solutions
if (life_count <= 0) {
return;
}
// try to solve it again
solve(goal_point, life_count-1);
} else {
*/
cout << "final error: " << curr_err << endl;
}
