void rpyFromQuat(const Quaterniond &q, double &roll, double &pitch, double &yaw)
{
Matrix3d r = q.toRotationMatrix() ;
Vector3d euler = r.eulerAngles(2, 1, 0) ;
yaw = euler.x() ;
pitch = euler.y() ;
roll = euler.z() ;
// pitch = atan2( 2*(q.y()*q.z() + q.w()*q.x()), q.w()*q.w() - q.x() * q.x() - q.y() * q.y() + q.z() * q.z()) ;
// yaw = asin(-2*(q.x()*q.z() - q.w()*q.y()) ) ;
// roll = atan2(2*(q.x()*q.y() + q.w()*q.z()), q.w()*q.w() + q.x() * q.x() - q.y() * q.y() - q.z() * q.z()) ;
}
void GLQuad::createMatrix( Quaterniond q, GLdouble *m )
{
Matrix3d m3 = q.toRotationMatrix();
m[0] = m3(0,0);
m[1] = m3(1,0);
m[2] = m3(2,0);
m[3] = 0;
m[4] = m3(0,1);
m[5] = m3(1,1);
m[6] = m3(2,1);
m[7] = 0;
m[8] = m3(0,2);
m[9] = m3(1,2);
m[10] = m3(2,2);
m[11] = 0;
m[12] = 0;
m[13] = 0;
m[14] = 0;
m[15] = 1;
}
void setupSBA(SysSBA &sys)
{
// Create camera parameters.
frame_common::CamParams cam_params;
cam_params.fx = 430; // Focal length in x
cam_params.fy = 430; // Focal length in y
cam_params.cx = 320; // X position of principal point
cam_params.cy = 240; // Y position of principal point
cam_params.tx = -30; // Baseline (no baseline since this is monocular)
// Define dimensions of the image.
int maxx = 640;
int maxy = 480;
// Create a plane containing a wall of points.
Plane middleplane;
middleplane.resize(3, 2, 10, 5);
middleplane.translate(0.0, 0.0, 7.0);
Plane leftplane;
leftplane.resize(1, 2, 6, 12);
// leftplane.rotate(-PI/4.0, 0, 1, 0);
leftplane.translate(0, 0, 7.0);
Plane rightplane;
rightplane.resize(1, 2, 6, 12);
// rightplane.rotate(PI/4.0, 0, 1, 0);
rightplane.translate(2, 0, 7.0);
Plane topplane;
topplane.resize(1, 1.5, 6, 12);
// topplane.rotate(PI/4.0, 1, 0, 0);
topplane.translate(2, 0, 7.0);
// Vector containing the true point positions.
rightplane.normal = rightplane.normal;
vector<Point, Eigen::aligned_allocator<Point> > points;
vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > normals;
points.insert(points.end(), middleplane.points.begin(), middleplane.points.end());
normals.insert(normals.end(), middleplane.points.size(), middleplane.normal);
points.insert(points.end(), leftplane.points.begin(), leftplane.points.end());
normals.insert(normals.end(), leftplane.points.size(), leftplane.normal);
points.insert(points.end(), rightplane.points.begin(), rightplane.points.end());
normals.insert(normals.end(), rightplane.points.size(), rightplane.normal);
points.insert(points.end(), topplane.points.begin(), topplane.points.end());
normals.insert(normals.end(), topplane.points.size(), topplane.normal);
// Create nodes and add them to the system.
unsigned int nnodes = 2; // Number of nodes.
double path_length = 0.5; // Length of the path the nodes traverse.
// Set the random seed.
unsigned short seed = (unsigned short)time(NULL);
seed48(&seed);
unsigned int i = 0, j = 0;
for (i = 0; i < nnodes; i++)
{
// Translate in the x direction over the node path.
Vector4d trans(i/(nnodes-1.0)*path_length, 0, 0, 1);
#if 1
if (i >= 0)
{
// perturb a little
double tnoise = 0.5; // meters
trans.x() += tnoise*(drand48()-0.5);
trans.y() += tnoise*(drand48()-0.5);
trans.z() += tnoise*(drand48()-0.5);
}
#endif
// Don't rotate.
Quaterniond rot(1, 0, 0, 0);
#if 1
if (i >= 0)
{
// perturb a little
double qnoise = 0.1; // meters
rot.x() += qnoise*(drand48()-0.5);
rot.y() += qnoise*(drand48()-0.5);
rot.z() += qnoise*(drand48()-0.5);
}
#endif
rot.normalize();
// Add a new node to the system.
sys.addNode(trans, rot, cam_params, false);
}
double pointnoise = 1.0;
// Add points into the system, and add noise.
for (i = 0; i < points.size(); i++)
{
// Add up to .5 points of noise.
Vector4d temppoint = points[i];
temppoint.x() += pointnoise*(drand48() - 0.5);
temppoint.y() += pointnoise*(drand48() - 0.5);
temppoint.z() += pointnoise*(drand48() - 0.5);
sys.addPoint(temppoint);
}
Vector2d proj2d;
Vector3d proj, pc, baseline;
Vector3d imagenormal(0, 0, 1);
Matrix3d covar0;
covar0 << sqrt(imagenormal(0)), 0, 0, 0, sqrt(imagenormal(1)), 0, 0, 0, sqrt(imagenormal(2));
Matrix3d covar;
Quaterniond rotation;
Matrix3d rotmat;
unsigned int midindex = middleplane.points.size();
unsigned int leftindex = midindex + leftplane.points.size();
unsigned int rightindex = leftindex + rightplane.points.size();
printf("Normal for Middle Plane: [%f %f %f], index %d -> %d\n", middleplane.normal.x(), middleplane.normal.y(), middleplane.normal.z(), 0, midindex-1);
printf("Normal for Left Plane: [%f %f %f], index %d -> %d\n", leftplane.normal.x(), leftplane.normal.y(), leftplane.normal.z(), midindex, leftindex-1);
printf("Normal for Right Plane: [%f %f %f], index %d -> %d\n", rightplane.normal.x(), rightplane.normal.y(), rightplane.normal.z(), leftindex, rightindex-1);
// Project points into nodes.
for (i = 0; i < points.size(); i++)
{
for (j = 0; j < sys.nodes.size(); j++)
{
// Project the point into the node's image coordinate system.
sys.nodes[j].setProjection();
sys.nodes[j].project2im(proj2d, points[i]);
// Camera coords for right camera
baseline << sys.nodes[j].baseline, 0, 0;
pc = sys.nodes[j].Kcam * (sys.nodes[j].w2n*points[i] - baseline);
proj.head<2>() = proj2d;
proj(2) = pc(0)/pc(2);
// If valid (within the range of the image size), add the stereo
// projection to SBA.
if (proj.x() > 0 && proj.x() < maxx && proj.y() > 0 && proj.y() < maxy)
{
sys.addStereoProj(j, i, proj);
// Create the covariance matrix:
// image plane normal = [0 0 1]
// wall normal = [0 0 -1]
// covar = (R)T*[0 0 0;0 0 0;0 0 1]*R
rotation.setFromTwoVectors(imagenormal, normals[i]);
rotmat = rotation.toRotationMatrix();
covar = rotmat.transpose()*covar0*rotmat;
// if (!(i % sys.nodes.size() == j))
// sys.setProjCovariance(j, i, covar);
}
}
}
// Add noise to node position.
double transscale = 2.0;
double rotscale = 0.2;
// Don't actually add noise to the first node, since it's fixed.
for (i = 1; i < sys.nodes.size(); i++)
{
Vector4d temptrans = sys.nodes[i].trans;
Quaterniond tempqrot = sys.nodes[i].qrot;
// Add error to both translation and rotation.
temptrans.x() += transscale*(drand48() - 0.5);
temptrans.y() += transscale*(drand48() - 0.5);
temptrans.z() += transscale*(drand48() - 0.5);
tempqrot.x() += rotscale*(drand48() - 0.5);
tempqrot.y() += rotscale*(drand48() - 0.5);
tempqrot.z() += rotscale*(drand48() - 0.5);
tempqrot.normalize();
sys.nodes[i].trans = temptrans;
sys.nodes[i].qrot = tempqrot;
// These methods should be called to update the node.
sys.nodes[i].normRot();
sys.nodes[i].setTransform();
sys.nodes[i].setProjection();
sys.nodes[i].setDr(true);
}
}
void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
{
// Dummy data as the real position for the system.
// static int old = msg->header.seq;
// if(msg->header.seq == old)
// filtered_pose.pose.position.z = 0;
// else{
// filtered_pose.pose.position.z += 10.0/70.0 * (msg->header.seq-old);
// }
// old = msg->header.seq;
ROS_INFO("Frame seq: %i", msg->header.seq);
#ifdef USE_ASE_IMU
tf::StampedTransform transform;
try{
listener_.lookupTransform("/camera", "/worldNED",
ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
return;
}
#endif
cv::Mat img;
try {
img = cv_bridge::toCvShare(msg, "mono8")->image;
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
}
processUserActions();
// Quaterniond quat(filtered_pose.pose.orientation.w,filtered_pose.pose.orientation.x,filtered_pose.pose.orientation.y, filtered_pose.pose.orientation.z);
// Matrix3d orient = quat.toRotationMatrix();
// Vector3d pos(filtered_pose.pose.position.x,filtered_pose.pose.position.y, filtered_pose.pose.position.z);
Quaterniond quat;
quat.setIdentity();
Vector3d pos;
pos.setZero();
Matrix3d orient = quat.toRotationMatrix();
#ifdef USE_ASE_IMU
tf::quaternionTFToEigen(transform.getRotation(), quat);
orient = quat.toRotationMatrix();
tf::vectorTFToEigen(transform.getOrigin(), pos);
#endif
vo_->addImage(img, msg->header.stamp.toSec(), orient, pos);
visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec());
if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED)
visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
if(publish_dense_input_)
visualizer_.exportToDense(vo_->lastFrame());
if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED)
usleep(100000);
}
const double astro::G = 6.672e-11; // N m^2 / kg^2
const double astro::SolarMass = 1.989e30;
const double astro::EarthMass = 5.976e24;
const double astro::LunarMass = 7.354e22;
const double astro::SOLAR_IRRADIANCE = 1367.6; // Watts / m^2
const double astro::SOLAR_POWER = 3.8462e26; // Watts
// Angle between J2000 mean equator and the ecliptic plane.
// 23 deg 26' 21".448 (Seidelmann, _Explanatory Supplement to the
// Astronomical Almanac_ (1992), eqn 3.222-1.
const double astro::J2000Obliquity = degToRad(23.4392911);
static const Quaterniond ECLIPTIC_TO_EQUATORIAL_ROTATION = XRotation(-astro::J2000Obliquity);
static const Matrix3d ECLIPTIC_TO_EQUATORIAL_MATRIX = ECLIPTIC_TO_EQUATORIAL_ROTATION.toRotationMatrix();
static const Quaterniond EQUATORIAL_TO_ECLIPTIC_ROTATION =
Quaterniond(AngleAxis<double>(-astro::J2000Obliquity, Vector3d::UnitX()));
static const Matrix3d EQUATORIAL_TO_ECLIPTIC_MATRIX = EQUATORIAL_TO_ECLIPTIC_ROTATION.toRotationMatrix();
static const Matrix3f EQUATORIAL_TO_ECLIPTIC_MATRIX_F = EQUATORIAL_TO_ECLIPTIC_MATRIX.cast<float>();
// Equatorial to galactic coordinate transformation
// North galactic pole at:
// RA 12h 51m 26.282s (192.85958 deg)
// Dec 27 d 07' 42.01" (27.1283361 deg)
// Zero longitude at position angle 122.932
// (J2000 coordinates)
static const double GALACTIC_NODE = 282.85958;
static const double GALACTIC_INCLINATION = 90.0 - 27.1283361;
static const double GALACTIC_LONGITUDE_AT_NODE = 32.932;