static double check_stop_cond_ADMM(struct DIAG_MAT *CT, struct DIAG_MAT *Einv, double *Cx, double *y, double *y_old, double *lambda, double *tmp_var_p, double *tmp_var_p2, double *tmp_var_n, int n, int p, double tol) {
double cond_num_p;
double cond_den_p;
double cond_p;
double cond_num_d;
double cond_den_d;
double cond_d;
mat_vec_mult_diag(Einv,Cx,tmp_var_p);
cond_den_p = norm_sq(tmp_var_p2,p);
cond_den_p = max(max(norm_sq(y,n),cond_den_p),1e-8);
vec_sub(tmp_var_p,y,tmp_var_p,p);
cond_num_p = norm_sq(tmp_var_p,p);
cond_p = pow2(tol)/4-cond_num_p/cond_den_p;
mat_vec_mult_diag(CT,lambda,tmp_var_n);
cond_den_d = max(norm_sq(tmp_var_n,n),1e-8);
vec_sub(y,y_old,tmp_var_p,p);
mat_vec_mult_diag(Einv,tmp_var_p,tmp_var_p2);
mat_vec_mult_diag(CT,tmp_var_p2,tmp_var_n);
cond_num_d = norm_sq(tmp_var_n,n);
cond_d = pow2(tol)/4-cond_num_d/cond_den_d;
return min(cond_p,cond_d);
}
static double check_stop_cond_FGM(struct DIAG_MAT *Einv, double *y, double *v, double *tmp_var_p, double *tmp_var_p2, int p, double tol) {
double cond_num;
double cond_den;
double cond;
vec_sub(y,v,tmp_var_p,p);
mat_vec_mult_diag(Einv,tmp_var_p,tmp_var_p2);
cond_num = norm_sq(tmp_var_p2,p);
mat_vec_mult_diag(Einv,y,tmp_var_p);
cond_den = norm_sq(tmp_var_p,p);
mat_vec_mult_diag(Einv,v,tmp_var_p);
cond_den = max(max(norm_sq(tmp_var_p,p),cond_den),1e-8);
cond = pow2(tol)-cond_num/cond_den;
return cond;
}
// Closest point to line segment between a and b (OK if a == b)
double distToLineSegment(point p, point a, point b, point &c) {
vec ap = toVec(a, p), ab = toVec(a, b);
double u = dot(ap, ab) / norm_sq(ab);
if (u < 0.0) { c = point(a.x, a.y); return dist(p, a); }
if (u > 1.0) { c = point(b.x, b.y); return dist(p, b); }
return distToLine(p, a, b, c);
}
// computes the orientation from (e1,e2,e3) -> (a,(a^b)^a,a^b), which means that b the second vector is in the a, b plane
static inline TooN::SO3<> canonicalOrientation( const TooN::Vector<3> & a, const TooN::Vector<3> & b ){
TooN::Vector<3> n = a ^ b;
if(norm_sq(n) < 1e-30)
return TooN::SO3<>();
TooN::Matrix<3> result;
result.T()[0] = unit(a);
result.T()[2] = unit(n);
result.T()[1] = result.T()[2] ^ result.T()[0];
return TooN::SO3<> (result);
}
double lr_event_rate (const VECTOR &dist, unsigned direction)
{
double rsq = norm_sq (dist);
if (rsq < sq (sr_lr_split))
return 0.;
double rate = unit_directional_derivative (dist[direction], rsq);
if (rate <= 0.)
return 0.;
else
return stren * rate;
}
double lr_event_rate (const VECTOR &r, size_t direction)
{
double rsq = norm_sq (r) / sq (scale);
if (rsq < sq (sr_lr_split) || rsq > sq (cutoff))
return 0.;
double rsix = pow (rsq, -3.);
double rate = (12.*rsix - 6.) * r[direction];
if (rate <= 0.)
return 0.;
else
return rate * strength * rsix / (scale * scale * rsq);
}
// Closest point to the line defined by a and b (must be different!)
double distToLine(point p, point a, point b, point &c) {
vec ap = toVec(a, p), ab = toVec(a, b);
double u = dot(ap, ab) / norm_sq(ab);
c = translate(a, scale(ab, u));
return dist(p, c);
}
double angle(point a, point o, point b) { // return angle aob in rad
vec oa = toVec(o, a), ob = toVec(o, b);
return acos(dot(a, ob) / sqrt(norm_sq(oa) * norm_sq(ob)));
}
void LoadXmlMap(const string& path,
Map& map,
bool all_frames) {
// Clear any old state
map.frames.clear();
map.points.clear();
// Configure the camera
map.orig_camera.reset(new ATANCamera(*gvDefaultCameraParams, *gvDefaultImageSize));
if (*gvLinearizeCamera) {
Mat3 cam = map.orig_camera->Linearize();
map.camera.reset(new LinearCamera(cam, map.orig_camera->image_size()));
} else {
map.camera = map.orig_camera;
}
// Parse the XML file
TiXmlDocument doc;
CHECK_PRED1(doc.LoadFile, path.c_str()) << "Failed to load " << path;
fs::path xml_dir(fs::path(path).parent_path());
fs::path frames_dir(*gvOrigFramesDir);
const TiXmlElement* root_elem = doc.RootElement();
// Read the points
std::map<int, int> points_id_to_index;
const TiXmlElement* points_elem = root_elem->FirstChildElement("MapPoints");
CHECK(points_elem) << "There was no <MapPoints> element in the map spec.";
for (const TiXmlElement* pt_elem = points_elem->FirstChildElement("MapPoint");
pt_elem != NULL;
pt_elem = pt_elem->NextSiblingElement("MapPoint")) {
int id = lexical_cast<int>(pt_elem->Attribute("id"));
points_id_to_index[id] = map.points.size(); // indices start from zero
map.points.push_back(stream_to<Vec3>(pt_elem->Attribute("position")));
}
// vector of frame IDs for which we fell back to B&W images
vector<int> fallback_frames;
// Read frame poses
int next_id = 0;
const TiXmlElement* frames_elem = root_elem->FirstChildElement("FramePoses");
if (all_frames) {
if (frames_elem != NULL) { // FramePoses is optional
for (const TiXmlElement* frame_elem = frames_elem->FirstChildElement("Frame");
frame_elem != NULL;
frame_elem = frame_elem->NextSiblingElement("Frame")) {
string image_file = (frames_dir/frame_elem->Attribute("name")).string();
Vec6 lnPose = stream_to<Vec6>(frame_elem->Attribute("pose"));
Frame* f = new Frame(next_id++, image_file, SE3<>::exp(lnPose));
f->initializing = norm_sq(lnPose) == 0;
f->lost = frame_elem->Attribute("lost") == "1" || f->initializing;
map.AddFrame(f);
}
}
} else {
// Read key frames
const TiXmlElement* kfs_elem = root_elem->FirstChildElement("KeyFrames");
CHECK(kfs_elem) << "There was no <KeyFrames> element in the map spec.";
// Create an ID-to-XMLElement map
for (const TiXmlElement* kf_elem = kfs_elem->FirstChildElement("KeyFrame");
kf_elem != NULL;
kf_elem = kf_elem->NextSiblingElement("KeyFrame")) {
// Get the id, pose, and hash
int id = lexical_cast<int>(kf_elem->Attribute("id"));
Vec6 lnPose = stream_to<Vec6>(kf_elem->Attribute("pose"));
string hash = kf_elem->FirstChildElement("Image")->Attribute("md5");
// Get the filename
fs::path image_file;
if (kf_elem->Attribute("name") != NULL) {
image_file = kf_elem->Attribute("name");
}
if (image_file.empty() || !*gvLoadOriginalFrames) {
fallback_frames.push_back(id);
image_file = xml_dir/kf_elem->FirstChildElement("Image")->Attribute("file");
} else {
image_file = frames_dir/image_file;
}
// Add the keyframe
Frame* frame = new Frame(id, image_file.string(), SE3<>::exp(lnPose));
map.AddFrame(frame);
// Add the measurements
const TiXmlElement* msms_elem = kf_elem->FirstChildElement("Measurements");
for (const TiXmlElement* msm_elem = msms_elem->FirstChildElement("Measurement");
msm_elem != NULL;
msm_elem = msm_elem->NextSiblingElement("Measurement")) {
Measurement msm;
int point_id = lexical_cast<int>(msm_elem->Attribute("id"));
std::map<int, int>::const_iterator it = points_id_to_index.find(point_id);
if (it == points_id_to_index.end()) {
DLOG << "No point with ID="<<point_id;
} else {
msm.point_index = it->second;
msm.image_pos = stream_to<Vec2>(msm_elem->Attribute("v2RootPos"));
msm.retina_pos = stream_to<Vec2>(msm_elem->Attribute("v2ImplanePos"));
msm.pyramid_level = lexical_cast<int>(msm_elem->Attribute("nLevel"));
}
frame->measurements.push_back(msm);
}
}
}
DLOG << "Loaded " << map.points.size() << " points and "
<< map.frames.size() << " frames";
}
double lr_event_rate (const vector <DIM> &r_, unsigned direction)
{
const double L = period[direction];
const vector <DIM> shift = unit_vector <DIM> (direction) * (L/2);
// assign polar pair partner
const double x_prim = r_[direction];
const double x_pair = x_prim - 2*L * floor (x_prim/L) - L;
bool first_copy = fabs (x_prim - x_pair) < 2*L;
vector <DIM> r[2] = { r_, r_ };
if (x_prim >= 0.)
r[0][direction] = x_pair;
else
r[1][direction] = x_pair;
double rsq[2] = { norm_sq (r[0]), norm_sq (r[1]) };
assert (r[0][direction] < 0.);
assert (r[1][direction] >= 0.);
// compute the rate for the polar pair
double rate = 0.;
// for large distances, the naive formula for the event rate can be
// numerically problematic.
if (fmax (rsq[0], rsq[1]) < 1e6*L*L)
{
for (int i = 0; i != 2; ++i)
{
double x = r[i][direction];
double u_rp = (i==0 && first_copy)
? 0.
: unit_potential (r[i] + shift);
double u_rm = (i==1 && first_copy)
? 0.
: unit_potential (r[i] - shift);
rate += (u_rp-u_rm) / L;
// main charge -- parts smaller than sr_lr_split are handled by the SR code
if (rsq[i] > sq (sr_lr_split))
rate += unit_directional_derivative (x, rsq[i]);
}
}
else if (fmax (rsq[0], rsq[1]) < 1e20*L*L)
{
for (int i = 0; i != 2; ++i)
{
double x = r[i][direction];
// avoid catastrophic cancellation in the far field
// (valid also for exponent = 0)
rate += x * pow (rsq[i], -.5*6. -.5*exponent) * L*L
* (2+exponent) * (1./24) * (3.*rsq[i] - (4+exponent)*x*x);
}
}
else
{
// rate < numerical precision
}
// asymptotic decay of particle event rate is ~ 1 / r^(n+4).
double w = (r[0][direction] == r_[direction])
? norm_sq (r[1]) / norm_sq (r[0])
: norm_sq (r[0]) / norm_sq (r[1]);
rate /= 1 + pow (w, -.5 * (exponent+4.));
if (rate <= 0.)
return 0.;
else
return stren * rate;
}
inline double norm (const Vector &v) {
return sqrt (norm_sq (v));
}
