/* ************************************************************************* */
Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2)
{
// get cosines and sines from rotation matrices
const Rot2& R1 = r1.r(), R2 = r2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = r2.t() - r1.t();
double x = dt.x(), y = dt.y();
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 = Matrix3(); (*H1) <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0;
}
if (H2) *H2 = Matrix3::Identity();
return Pose2(R,t);
}
double VectorMeters2NorthHeading(const Point2& ref, const Point2& p, const Vector2& dir)
{
double h = atan2(dir.dx, dir.dy) * RAD_TO_DEG;
if (h < 0.0) h += 360.0;
double lon_delta = p.x() - ref.x();
return h + lon_delta * sin(p.y() * DEG_TO_RAD);
}
pscalar PointSolver::LineSegment2D::errorJacobian (Point2 &P, pscalar *jm)
{
pscalar lsegmentsq = this->lengthSquared();
pscalar dep1x, dep1y, dep2x, dep2y;
pscalar p1x = A.coord.x(),
p1y = A.coord.y(),
p2x = B.coord.x(),
p2y = B.coord.y(),
px = P.x(), py = P.y();
dep1x = -2*(p2y-p1y)*
(p2x*py-p1x*py-p2y*px+p1y*px+p1x*p2y-p1y*p2x) *
(p2y*py-p1y*py+p2x*px-p1x*px-p2y*p2y+p1y*p2y-p2x*p2x+p1x*p2x) /
(lsegmentsq * lsegmentsq);
dep1y = 2*(p2x-p1x)*
(p2x*py-p1x*py-p2y*px+p1y*px+p1x*p2y-p1y*p2x) *
(p2y*py-p1y*py+p2x*px-p1x*px-p2y*p2y+p1y*p2y-p2x*p2x+p1x*p2x) /
(lsegmentsq * lsegmentsq);
dep2x = 2*(p2y-p1y)*
(p2x*py-p1x*py-p2y*px+p1y*px+p1x*p2y-p1y*p2x) *
(p2y*py-p1y*py+p2x*px-p1x*px-p1y*p2y-p1x*p2x+p1y*p1y+p1x*p1x) /
(lsegmentsq * lsegmentsq);
dep2y = -2*(p2x-p1x)*
(p2x*py-p1x*py-p2y*px+p1y*px+p1x*p2y-p1y*p2x)*
(p2y*py-p1y*py+p2x*px-p1x*px-p1y*p2y-p1x*p2x+p1y*p1y+p1x*p1x) /
(lsegmentsq * lsegmentsq);
for (int i=0; i<7; i++) {
jm[i] = dep1x*A.jacobian[i][0] + dep1y*A.jacobian[i][1] + dep2x*B.jacobian[i][0] + dep2y*B.jacobian[i][1];
}
}
Matrix H(const Point2& x_t1) const {
// Update Jacobian
Matrix H(1,2);
H(0,0) = 4*x_t1.x() - x_t1.y() - 2.5;
H(0,1) = -1*x_t1.x() + 7;
return H;
}
double ComputeRayAngle(Point2 p, Point2 q, const Camera& cam1, const Camera& cam2)
{
const Point3 p3n = cam1.GetIntrinsicMatrix().inverse() * EuclideanToHomogenous(p);
const Point3 q3n = cam2.GetIntrinsicMatrix().inverse() * EuclideanToHomogenous(q);
const Point2 pn = p3n.head<2>() / p3n.z();
const Point2 qn = q3n.head<2>() / q3n.z();
const Point3 p_w = cam1.m_R.transpose() * Point3{pn.x(), pn.y(), -1.0};
const Point3 q_w = cam2.m_R.transpose() * Point3{qn.x(), qn.y(), -1.0};
// Compute the angle between the rays
const double dot = p_w.dot(q_w);
const double mag = p_w.norm() * q_w.norm();
return acos(util::clamp((dot / mag), (-1.0 + 1.0e-8), (1.0 - 1.0e-8)));
}
void NorthHeading2VectorMeters(const Point2& ref, const Point2& p, double heading, Vector2& dir)
{
double lon_delta = p.x() - ref.x();
double real_heading = heading - lon_delta * sin(p.y() * DEG_TO_RAD);
dir.dx = sin(real_heading * DEG_TO_RAD);
dir.dy = cos(real_heading * DEG_TO_RAD);
}
/// \brief Constructor
///
/// @param a one end of the line defining the edge.
/// @param b one end of the line defining the edge.
Edge(const Point2& a, const Point2& b)
{
// horizontal segments should be discarded earlier
assert(a.y() != b.y());
if (a.y() < b.y()) {
m_start = a;
m_seg = b - a;
} else {
m_start = b;
m_seg = a - b;
}
// normal gradient is y/x, here we use x/y. seg.y() will be != 0,
// as we already asserted above.
m_inverseGradient = m_seg.x() / m_seg.y();
}
void
ShortestPathFinder::FindPathToEndPoint ( Point2<int> const& iEndPoint,
std::list<Point2<int> >& ioPoints ) {
if( iEndPoint.x() < 0 || iEndPoint.x() >= mzX ||
iEndPoint.y() < 0 || iEndPoint.y() >= mzY )
throw runtime_error( "Invalid start point, out of bounds." );
// Find the costs if we haven't already.
if( !mbValidCosts )
FindCosts();
// Start at the end and follow the direction table back to the
// beginning, adding points on the way. The next step in the
// shortest path is the neighbor with the lowest cost.
ioPoints.clear();
Point2<int> current( iEndPoint );
while ( current.x() != mStartPoint.x() ||
current.y() != mStartPoint.y() ) {
// Push the current point to the result path.
ioPoints.push_front( current );
// Search for the lowest cost of its neighbors.
float lowestCost = numeric_limits<float>::max();
int lowestDirection = 0;
for ( int direction = 1; direction < 9; direction++ ) {
Point2<int> neighbor( current.x() + GetDirectionOffset(direction)[0],
current.y() + GetDirectionOffset(direction)[1]);
if ( neighbor.x() >= 0 && neighbor.x() < mzX &&
neighbor.y() >= 0 && neighbor.y() < mzY ) {
float cost = maTotalCost->Get( neighbor );
if( cost < lowestCost ) {
lowestCost = cost;
lowestDirection = direction;
}
}
}
// Update current to be the neighbor with the lowest cost.
current.Set( current.x() + GetDirectionOffset(lowestDirection)[0],
current.y() + GetDirectionOffset(lowestDirection)[1]);
}
}
void MetersToLLE(const Point2& ref, int count, Point2 * pts)
{
while(count--)
{
pts->y_ = ref.y() + pts->y() * MTR_TO_DEG_LAT;
pts->x_ = ref.x() + pts->x() * MTR_TO_DEG_LAT / cos(pts->y() * DEG_TO_RAD);
++pts;
}
}
bool WED_GISBoundingBox::PtOnFrame (GISLayer_t l,const Point2& p, double d) const
{
Bbox2 me;
GetBounds(l,me);
if (p.x() < me.xmin() ||
p.x() > me.xmax() ||
p.y() < me.ymin() ||
p.y() > me.ymax())
return false;
if (p.x() > me.xmin() &&
p.x() < me.xmax() &&
p.y() > me.ymin() &&
p.y() < me.ymax())
return false;
return true;
}
// Calculate the next state prediction using the nonlinear function f()
Point2 f(const Point2& x_t0) const {
// Calculate f(x)
double x = x_t0.x() * x_t0.x();
double y = x_t0.x() * x_t0.y();
Point2 x_t1(x,y);
// In this example, the noiseModel remains constant
return x_t1;
}
// Calculate the Jacobian of the nonlinear function f()
Matrix F(const Point2& x_t0) const {
// Calculate Jacobian of f()
Matrix F = Matrix(2,2);
F(0,0) = 2*x_t0.x();
F(0,1) = 0.0;
F(1,0) = x_t0.y();
F(1,1) = x_t0.x();
return F;
}
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point2 q = r_ * p;
if (H1 || H2) {
const Matrix R = r_.matrix();
const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x());
if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
if (H2) *H2 = R; // R
}
return q + t_;
}
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 = (Matrix(2, 3) <<
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x());
if (H2) *H2 = r_.transpose();
return q;
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ;
const double r = xx + yy ;
const double r2 = r*r ;
const double f = f_ ;
const double g = 1 + (k1_+k2_*r) * r ; // save one multiply
//const double g = (1+k1_*r+k2_*r2) ;
return Matrix_(2, 3,
g*x, f*x*r , f*x*r2,
g*y, f*y*r , f*y*r2);
}
/* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) {
const Rot2& R = p.r();
const Point2& t = p.t();
double w = R.theta();
if (std::abs(w) < 1e-10)
return (Vector(3) << t.x(), t.y(), w);
else {
double c_1 = R.c()-1.0, s = R.s();
double det = c_1*c_1 + s*s;
Point2 p = R_PI_2 * (R.unrotate(t) - t);
Point2 v = (w/det) * p;
return (Vector(3) << v.x(), v.y(), w);
}
}
/* ************************************************************************* */
TEST( Cal3DS2, uncalibrate)
{
Vector k = K.k() ;
double r = p.x()*p.x() + p.y()*p.y() ;
double g = 1+k[0]*r+k[1]*r*r ;
double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ;
double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ;
Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0) ;
Vector v_i = K.K() * v_hat ;
Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
Point2 q = K.uncalibrate(p);
CHECK(assert_equal(q,p_i));
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
const double r = xx + yy ;
const double dr_dx = 2*x ;
const double dr_dy = 2*y ;
const double g = 1 + (k1_+k2_*r) * r ; // save one multiply
//const double g = 1 + k1_*r + k2_*r*r ;
const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ;
const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ;
Matrix DK = Matrix_(2, 2, f_, 0.0, 0.0, f_);
Matrix DR = Matrix_(2, 2,
g + x*dg_dx, x*dg_dy,
y*dg_dx , g + y*dg_dy) ;
return DK * DR;
}
static inline bool point_is_spike_or_equal(Point1 const& last_point, Point2 const& segment_a, Point3 const& segment_b)
{
// adapted from boost\geometry\algorithms\detail\point_is_spike_or_equal.hpp to include tolerance checking
// segment_a is at the beginning
// segment_b is in the middle
// last_point is at the end
// segment_b is being considered for deletion
double normTol = 0.001; // 1 mm
double tol = 0.001; // relative to 1
double diff1_x = last_point.x()-segment_b.x();
double diff1_y = last_point.y()-segment_b.y();
double norm1 = sqrt(pow(diff1_x, 2) + pow(diff1_y, 2));
if (norm1 > normTol){
diff1_x = diff1_x/norm1;
diff1_y = diff1_y/norm1;
}else{
// last point is too close to segment b
return true;
}
double diff2_x = segment_b.x()-segment_a.x();
double diff2_y = segment_b.y()-segment_a.y();
double norm2 = sqrt(pow(diff2_x, 2) + pow(diff2_y, 2));
if (norm2 > normTol){
diff2_x = diff2_x/norm2;
diff2_y = diff2_y/norm2;
}else{
// segment b is too close to segment a
return true;
}
double crossProduct = diff1_x*diff2_y-diff1_y*diff2_x;
if (abs(crossProduct) < tol){
double dotProduct = diff1_x*diff2_x+diff1_y*diff2_y;
if (dotProduct <= -1.0 + tol){
// reversal
return true;
}
}
return false;
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
const double r = xx + yy ;
const double r2 = r*r;
const double dr_dx = 2*x ;
const double dr_dy = 2*y ;
const double g = 1 + (k1_*r) + (k2_*r2) ;
const double f = f_ ;
const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ;
const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ;
Matrix H = Matrix_(2,5,
f*(g + x*dg_dx), f*(x*dg_dy), g*x, f*x*r , f*x*r2,
f*(y*dg_dx) , f*(g + y*dg_dy), g*y, f*y*r , f*y*r2);
return H ;
}
Point2 Exact_adaptive_kernel::circumcenter(Point2 const& p1, Point2 const& p2, Point2 const& p3)
{
Point2 p2p1(p2.x()-p1.x(), p2.y()-p1.y());
Point2 p3p1(p3.x()-p1.x(), p3.y()-p1.y());
Point2 p2p3(p2.x()-p3.x(), p2.y()-p3.y());
double p2p1dist = p2p1.x()*p2p1.x() + p2p1.y()*p2p1.y();
double p3p1dist = p3p1.x()*p3p1.x() + p3p1.y()*p3p1.y();
double denominator = 0.5/(2.0*signed_area(p1, p2, p3));
BOOST_ASSERT(denominator > 0.0);
double dx = (p3p1.y() * p2p1dist - p2p1.y() * p3p1dist) * denominator;
double dy = (p2p1.x() * p3p1dist - p3p1.x() * p2p1dist) * denominator;
return Point2(p1.x()+dx, p1.y()+dy);
}
bool IsInsideTriangle(const ISectTriangle* triangle, MPMesh* pMesh, MPMesh::FaceHandle coTri, const Point2 &bc, Relation &rel)
{
Vec3d *v0, *v1, *v2;
GetCorners(pMesh, coTri, v0, v1, v2);
OpenMesh::Vec2d v[3];
v[0][0] = (*v0)[triangle->xi]; v[0][1] = (*v0)[triangle->yi];
v[1][0] = (*v1)[triangle->xi]; v[1][1] = (*v1)[triangle->yi];
v[2][0] = (*v2)[triangle->xi]; v[2][1] = (*v2)[triangle->yi];
if (IsPointInTriangle(OpenMesh::Vec2d(bc.x(), bc.y()), v, rel))
{
double d = OpenMesh::dot(triangle->pMesh->normal(triangle->face), pMesh->normal(coTri));
if (pMesh->bInverse ^ triangle->pMesh->bInverse) d = -d;
if (d > 0.0) rel = REL_SAME;
else rel = REL_OPPOSITE;
return true;
}
else return false;
}
void Object::getChildrenDimensions(Point2& r_pos, Point2& r_size) {
Point2 min(0, 0);
Point2 max = size;
if(hasChildren()) {
BOOST_FOREACH(const PTR(Object)& a, children) {
Point2 p, s;
a->getChildrenDimensions(p, s);
float x1 = p.x();
float x2 = p.x() + s.x();
float y1 = p.y();
float y2 = p.y() + s.y();
Danvilifmin(min.x(), x1);
Danvilifmax(max.x(), x2);
Danvilifmin(min.y(), y1);
Danvilifmax(max.y(), y2);
}
}
r_pos = min + pos;
r_size = max - min;
}
/* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
Point3 q = pose_.transform_to(point, H1, H2);
#else
Point3 q = pose_.transform_to(point);
#endif
Point2 intrinsic = project_to_camera(q);
// Check if point is in front of camera
if(q.z() <= 0)
throw CheiralityException();
if (H1 || H2) {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
// just implement chain rule
Matrix H;
project_to_camera(q,H);
if (H1) *H1 = H * (*H1);
if (H2) *H2 = H * (*H2);
#else
// optimized version, see CalibratedCamera.nb
const double z = q.z(), d = 1.0/z;
const double u = intrinsic.x(), v = intrinsic.y(), uv = u*v;
if (H1) *H1 = Matrix_(2,6,
uv,-(1.+u*u), v, -d , 0., d*u,
(1.+v*v), -uv,-u, 0.,-d , d*v
);
if (H2) {
const Matrix R(pose_.rotation().matrix());
*H2 = d * Matrix_(2,3,
R(0,0) - u*R(0,2), R(1,0) - u*R(1,2), R(2,0) - u*R(2,2),
R(0,1) - v*R(0,2), R(1,1) - v*R(1,2), R(2,1) - v*R(2,2)
);
}
#endif
}
return intrinsic;
}
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// r = x^2 + y^2 ;
// g = (1 + k(1)*r + k(2)*r^2) ;
// pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y() ;
const double r = x*x + y*y ;
const double r2 = r*r ;
const double g = 1 + k1_ * r + k2_*r2 ; // save one multiply
const double fg = f_*g ;
// semantic meaningful version
//if (H1) *H1 = D2d_calibration(p) ;
//if (H2) *H2 = D2d_intrinsic(p) ;
// unrolled version, much faster
if ( H1 || H2 ) {
const double fx = f_*x, fy = f_*y ;
if ( H1 ) {
*H1 = Matrix_(2, 3,
g*x, fx*r , fx*r2,
g*y, fy*r , fy*r2) ;
}
if ( H2 ) {
const double dr_dx = 2*x ;
const double dr_dy = 2*y ;
const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ;
const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ;
*H2 = Matrix_(2, 2,
fg + fx*dg_dx, fx*dg_dy,
fy*dg_dx , fg + fy*dg_dy) ;
}
}
return Point2(fg * x, fg * y) ;
}
/* ************************************************************************* */
bool SimPolygon2D::contains(const Point2& c) const {
vector<SimWall2D> edges = walls();
bool initialized = false;
bool lastSide = false;
BOOST_FOREACH(const SimWall2D& ab, edges) {
// compute cross product of ab and ac
Point2 dab = ab.b() - ab.a();
Point2 dac = c - ab.a();
double cross = dab.x() * dac.y() - dab.y() * dac.x();
if (fabs(cross) < 1e-6) // check for on one of the edges
return true;
bool side = cross > 0;
// save the first side found
if (!initialized) {
lastSide = side;
initialized = true;
continue;
}
// to be inside the polygon, point must be on the same side of all lines
if (lastSide != side)
return false;
}
Point2 Exact_adaptive_kernel::offcenter(Point2 const& p1, Point2 const& p2, Point2 const& p3, double offconstant)
{
Point2 p2p1(p2.x()-p1.x(), p2.y()-p1.y());
Point2 p3p1(p3.x()-p1.x(), p3.y()-p1.y());
Point2 p2p3(p2.x()-p3.x(), p2.y()-p3.y());
double p2p1dist = p2p1.x()*p2p1.x() + p2p1.y()*p2p1.y();
double p3p1dist = p3p1.x()*p3p1.x() + p3p1.y()*p3p1.y();
double p2p3dist = p2p3.x()*p2p3.x() + p2p3.y()*p2p3.y();
double denominator = 0.5/(2.0*Exact_adaptive_kernel::signed_area(p1, p2, p3));
BOOST_ASSERT(denominator > 0.0);
double dx = (p3p1.y() * p2p1dist - p2p1.y() * p3p1dist) * denominator;
double dy = (p2p1.x() * p3p1dist - p3p1.x() * p2p1dist) * denominator;
double dxoff, dyoff;
if ((p2p1dist < p3p1dist) && (p2p1dist < p2p3dist)) {
dxoff = 0.5 * p2p1.x() - offconstant * p2p1.y();
dyoff = 0.5 * p2p1.y() + offconstant * p2p1.x();
if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy) {
dx = dxoff;
dy = dyoff;
}
} else if (p3p1dist < p2p3dist) {
dxoff = 0.5 * p3p1.x() + offconstant * p3p1.y();
dyoff = 0.5 * p3p1.y() - offconstant * p3p1.x();
if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy) {
dx = dxoff;
dy = dyoff;
}
} else {
dxoff = 0.5 * p2p3.x() - offconstant * p2p3.y();
dyoff = 0.5 * p2p3.y() + offconstant * p2p3.x();
if (dxoff * dxoff + dyoff * dyoff < (dx - p2p1.x()) * (dx - p2p1.x()) + (dy - p2p1.y()) * (dy - p2p1.y())) {
dx = p2p1.x() + dxoff;
dy = p2p1.y() + dyoff;
}
}
return Point2(p1.x()+dx, p1.y()+dy);
}
// main
int main(int argc, char** argv) {
// load Plaza1 data
list<TimedOdometry> odometry = readOdometry();
// size_t M = odometry.size();
vector<RangeTriple> triples = readTriples();
size_t K = triples.size();
// parameters
size_t start = 220, end=3000;
size_t minK = 100; // first batch of smart factors
size_t incK = 50; // minimum number of range measurements to process after
bool robust = true;
bool smart = true;
// Set Noise parameters
Vector priorSigmas = Vector3(1, 1, M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
double sigmaR = 100; // range standard deviation
const NM::Base::shared_ptr // all same type
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
rangeNoise = robust ? tukey : gaussian;
// Initialize iSAM
ISAM2 isam;
// Add prior on first pose
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000);
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
ofstream os2(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
ofstream os3(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultSR.txt");
// initialize points (Gaussian)
Values initial;
if (!smart) {
initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
}
Values landmarkEstimates = initial; // copy landmarks
initial.insert(0, pose0);
// initialize smart range factors
size_t ids[] = { 1, 6, 0, 5 };
typedef boost::shared_ptr<SmartRangeFactor> SmartPtr;
map<size_t, SmartPtr> smartFactors;
if (smart) {
for(size_t jj: ids) {
smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR));
newFactors.push_back(smartFactors[jj]);
}
}
// set some loop variables
size_t i = 1; // step counter
size_t k = 0; // range measurement counter
Pose2 lastPose = pose0;
size_t countK = 0, totalCount=0;
// Loop over odometry
gttic_(iSAM);
for(const TimedOdometry& timedOdometry: odometry) {
//--------------------------------- odometry loop -----------------------------------------
double t;
Pose2 odometry;
boost::tie(t, odometry) = timedOdometry;
// add odometry factor
newFactors.push_back(
BetweenFactor<Pose2>(i - 1, i, odometry,
NM::Diagonal::Sigmas(odoSigmas)));
// predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry);
lastPose = predictedPose;
initial.insert(i, predictedPose);
landmarkEstimates.insert(i, predictedPose);
// Check if there are range factors to be added
while (k < K && t >= boost::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]);
double range = boost::get<2>(triples[k]);
if (i > start) {
if (smart && totalCount < minK) {
smartFactors[j]->addRange(i, range);
printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl;
}
else {
RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,
rangeNoise);
// Throw out obvious outliers based on current landmark estimates
Vector error = factor.unwhitenedError(landmarkEstimates);
if (k <= 200 || fabs(error[0]) < 5)
newFactors.push_back(factor);
}
totalCount += 1;
}
k = k + 1;
countK = countK + 1;
}
// Check whether to update iSAM 2
if (k >= minK && countK >= incK) {
gttic_(update);
isam.update(newFactors, initial);
gttoc_(update);
gttic_(calculateEstimate);
Values result = isam.calculateEstimate();
gttoc_(calculateEstimate);
lastPose = result.at<Pose2>(i);
bool hasLandmarks = result.exists(symbol('L', ids[0]));
if (hasLandmarks) {
// update landmark estimates
landmarkEstimates = Values();
for(size_t jj: ids)
landmarkEstimates.insert(symbol('L', jj), result.at(symbol('L', jj)));
}
newFactors = NonlinearFactorGraph();
initial = Values();
if (smart && !hasLandmarks) {
cout << "initialize from smart landmarks" << endl;
for(size_t jj: ids) {
Point2 landmark = smartFactors[jj]->triangulate(result);
initial.insert(symbol('L', jj), landmark);
landmarkEstimates.insert(symbol('L', jj), landmark);
}
}
countK = 0;
for(const Values::ConstFiltered<Point2>::KeyValuePair& it: result.filter<Point2>())
os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
<< endl;
if (smart) {
for(size_t jj: ids) {
Point2 landmark = smartFactors[jj]->triangulate(result);
os3 << jj << "\t" << landmark.x() << "\t" << landmark.y() << "\t1"
<< endl;
}
}
}
i += 1;
if (i>end) break;
//--------------------------------- odometry loop -----------------------------------------
} // end for
gttoc_(iSAM);
// Print timings
tictoc_print_();
// Write result to file
Values result = isam.calculateEstimate();
ofstream os(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>())
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
<< it.value.theta() << endl;
exit(0);
}
/* ************************************************************************* */
Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) {
return Point3(p.x() * scale, p.y() * scale, scale);
}
/******************************************************************************
* Renders the text string at the given location given in normalized
* viewport coordinates ([-1,+1] range).
******************************************************************************/
void DefaultTextPrimitive::renderViewport(SceneRenderer* renderer, const Point2& pos, int alignment)
{
QSize imageSize = renderer->outputSize();
Point2 windowPos((pos.x() + 1.0f) * imageSize.width() / 2, (-pos.y() + 1.0f) * imageSize.height() / 2);
renderWindow(renderer, windowPos, alignment);
}
