bool VertexCam::read(std::istream& is)
{
// first the position and orientation (vector3 and quaternion)
Vector3d t;
for (int i=0; i<3; i++){
is >> t[i];
}
Vector4d rc;
for (int i=0; i<4; i++) {
is >> rc[i];
}
Quaterniond r;
r.coeffs() = rc;
r.normalize();
// form the camera object
SBACam cam(r,t);
// now fx, fy, cx, cy, baseline
double fx, fy, cx, cy, tx;
// try to read one value
is >> fx;
if (is.good()) {
is >> fy >> cx >> cy >> tx;
cam.setKcam(fx,fy,cx,cy,tx);
} else{
inline Pose::Pose(const Quaterniond& orientation,
const Vector3d& position)
: m_orientation(orientation)
, m_position(position)
{
CHECK(std::fabs(orientation.norm() - 1) < 1e-15)
<< "Your quaternion is not normalized:"
<< orientation.norm();
// Or, one may prefer
m_orientation.normalize();
}
TEST(OsgRigidTransformConversionsTests, RigidTransform3dTest)
{
Quaterniond rotation = Quaterniond(Vector4d::Random());
rotation.normalize();
Vector3d translation = Vector3d::Random();
RigidTransform3d transform = makeRigidTransform(rotation, translation);
/// Convert to OSG
std::pair<osg::Quat, osg::Vec3d> osgTransform = toOsg(transform);
/// Convert back to Eigen and compare with original
RigidTransform3d resultTransform = fromOsg(osgTransform);
EXPECT_TRUE(transform.isApprox(resultTransform));
}
TEST(OsgRigidTransformConversionsTests, RigidTransform3dMultiplyTest)
{
Quaterniond rotation = Quaterniond(Vector4d::Random());
rotation.normalize();
Vector3d translation = Vector3d::Random();
RigidTransform3d transform = makeRigidTransform(rotation, translation);
Vector3d vector = Vector3d::Random();
osg::Vec3d osgVector = toOsg(vector);
/// Transform the vector using Eigen
Vector3d result = transform * vector;
std::pair<osg::Quat, osg::Vec3d> osgTransform = toOsg(transform);
/// Transform the vector using OSG
osg::Vec3d osgResult = osgTransform.first * osgVector + osgTransform.second;
/// Compare the transformations
EXPECT_TRUE(result.isApprox(fromOsg(osgResult)));
}
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 = 0; // 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.
int npts_x = 10; // Number of points on the plane in x
int npts_y = 5; // Number of points on the plane in y
double plane_width = 5; // Width of the plane on which points are positioned (x)
double plane_height = 2.5; // Height of the plane on which points are positioned (y)
double plane_distance = 5; // Distance of the plane from the cameras (z)
// Vector containing the true point positions.
vector<Point, Eigen::aligned_allocator<Point> > points;
for (int ix = 0; ix < npts_x ; ix++)
{
for (int iy = 0; iy < npts_y ; iy++)
{
// Create a point on the plane in a grid.
points.push_back(Point(plane_width/npts_x*(ix+.5), -plane_height/npts_y*(iy+.5), plane_distance, 1.0));
}
}
// Create nodes and add them to the system.
unsigned int nnodes = 5; // Number of nodes.
double path_length = 3; // Length of the path the nodes traverse.
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);
// Don't rotate.
Quaterniond rot(1, 0, 0, 0);
rot.normalize();
// Add a new node to the system.
sys.addNode(trans, rot, cam_params, false);
}
// Set the random seed.
unsigned short seed = (unsigned short)time(NULL);
seed48(&seed);
double ptscale = 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() += ptscale*(drand48() - 0.5);
temppoint.y() += ptscale*(drand48() - 0.5);
temppoint.z() += ptscale*(drand48() - 0.5);
sys.addPoint(temppoint);
}
Vector2d proj;
// 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(proj, points[i]);
// If valid (within the range of the image size), add the monocular
// projection to SBA.
if (proj.x() > 0 && proj.x() < maxx-1 && proj.y() > 0 && proj.y() < maxy-1)
{
sys.addMonoProj(j, i, proj);
//printf("Adding projection: Node: %d Point: %d Proj: %f %f\n", j, i, proj.x(), proj.y());
}
}
}
// Add noise to node position.
double transscale = 1.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);
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "handeye_calibration");
ros::NodeHandle nh ;
srand(clock()) ;
int c = 1 ;
while ( c < argc )
{
if ( strncmp(argv[c], "--camera", 8) == 0 )
{
if ( c + 1 < argc ) {
camera_id = argv[++c] ;
}
}
else if ( strncmp(argv[c], "--out", 5) == 0 )
{
if ( c + 1 < argc ) {
outFolder = argv[++c] ;
}
}
else if ( strncmp(argv[c], "--data", 6) == 0 )
{
if ( c + 1 < argc ) {
dataFolder = argv[++c] ;
}
}
else if ( strncmp(argv[c], "--arm", 5) == 0 )
{
if ( c + 1 < argc ) {
armName = argv[++c] ;
}
}
else if ( strncmp(argv[c], "--board", 7) == 0 )
{
int bx = -1, by = -1 ;
if ( c + 1 < argc ) {
bx = atof(argv[++c]) ;
}
if ( c + 1 < argc ) {
by = atof(argv[++c]) ;
}
if ( bx > 0 && by > 0 )
boardSize = cv::Size(bx, by) ;
}
else if ( strncmp(argv[c], "--cell", 7) == 0 )
{
if ( c + 1 < argc ) {
string cs = argv[++c] ;
cellSize = atof(cs.c_str()) ;;
}
}
else if ( strncmp(argv[c], "--stations", 10) == 0 )
{
if ( c + 1 < argc ) {
nStations = atoi(argv[++c]) ;
}
}
else if ( strncmp(argv[c], "--bbox", 6) == 0 )
{
if ( c + 1 < argc ) {
minX = atof(argv[++c]) ;
}
if ( c + 1 < argc ) {
minY = atof(argv[++c]) ;
}
if ( c + 1 < argc ) {
minZ = atof(argv[++c]) ;
}
if ( c + 1 < argc ) {
maxX = atof(argv[++c]) ;
}
if ( c + 1 < argc ) {
maxY = atof(argv[++c]) ;
}
if ( c + 1 < argc ) {
maxZ = atof(argv[++c]) ;
}
}
else if ( strncmp(argv[c], "--orient", 8) == 0 )
{
double ox, oy, oz ;
if ( c + 1 < argc ) {
ox = atof(argv[++c]) ;
}
if ( c + 1 < argc ) {
oy = atof(argv[++c]) ;
}
if ( c + 1 < argc ) {
oz = atof(argv[++c]) ;
}
orient = Vector3d(ox, oy, oz) ;
}
else if ( strncmp(argv[c], "--fixed", 7) == 0 )
{
fixedCam = true ;
}
++c ;
}
if ( camera_id.empty() )
{
ROS_ERROR("No camera specified") ;
return 0 ;
}
if ( outFolder.empty() )
{
outFolder = "/home/" ;
outFolder += getenv("USER") ;
outFolder += "/.ros/clopema_calibration/" ;
outFolder += camera_id ;
outFolder += "/handeye_rgb.calib" ;
}
ros::AsyncSpinner spinner(4) ;
spinner.start() ;
// open grabber and wait until connection
camera_helpers::OpenNICaptureRGBD grabber(camera_id) ;
if ( !grabber.connect() )
{
ROS_ERROR("Cannot connect to frame grabber: %s", camera_id.c_str()) ;
return 0 ;
}
c = 0 ;
// move robot and grab images
robot_helpers::MoveRobot mv ;
mv.setServoMode(false);
tf::TransformListener listener(ros::Duration(1.0));
double cx, cy, fx, fy ;
while ( c < nStations )
{
// move the robot to the next position
double X = minX + (maxX - minX)*(rand()/(double)RAND_MAX) ;
double Y = minY + (maxY - minY)*(rand()/(double)RAND_MAX) ;
double Z = minZ + (maxZ - minZ)*(rand()/(double)RAND_MAX) ;
const double qscale = 0.3 ;
double qx = qscale * (rand()/(double)RAND_MAX - 0.5) ;
double qy = qscale * (rand()/(double)RAND_MAX - 0.5) ;
double qz = qscale * (rand()/(double)RAND_MAX - 0.5) ;
double qw = qscale * (rand()/(double)RAND_MAX - 0.5) ;
Quaterniond q = robot_helpers::lookAt(orient, M_PI) ;
q = Quaterniond(q.x() + qx, q.y() + qy, q.z() + qz, q.w() + qw) ;
q.normalize();
if ( fixedCam ) {
addPlaneToCollisionModel(armName, 0.3, q) ;
if ( !robot_helpers::moveGripper(mv, armName, Eigen::Vector3d(X, Y, Z), q) ) continue ;
robot_helpers::resetCollisionModel() ;
}
else
{
// for an Xtion on arm the bounding box indicates the target position and the orient the lookAt direction
// so move back the sensor far enough so that the target is visible. This is currently hardcoded.
Vector3d dir = q*Vector3d(0, 0, 1) ;
Vector3d c = Vector3d(X, Y, Z) - 0.9*dir ;
trajectory_msgs::JointTrajectory traj ;
if ( !robot_helpers::planXtionToPose(armName, c, q, traj) ) continue ;
mv.execTrajectory(traj) ;
}
image_geometry::PinholeCameraModel cm ;
cv::Mat clr, depth ;
ros::Time ts ;
grabber.grab(clr, depth, ts, cm) ;
// camera intrinsics. we assume a rectfied and calibrated frame
cx = cm.cx() ;
cy = cm.cy() ;
fx = cm.fx() ;
fy = cm.fy() ;
string filenamePrefix = dataFolder + str(boost::format("/grab_%06d") % c) ;
cv::imwrite(filenamePrefix + "_c.png", clr) ;
cv::imwrite(filenamePrefix + "_d.png", depth) ;
Eigen::Affine3d pose_ ;
if ( fixedCam ) pose_ = robot_helpers::getPose(armName) ;
else
{
tf::StampedTransform transform;
try {
listener.waitForTransform(armName + "_link_6", "base_link", ts, ros::Duration(1) );
listener.lookupTransform(armName + "_link_6", "base_link", ts, transform);
tf::TransformTFToEigen(transform, pose_);
} catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
continue ;
}
}
{
ofstream strm((filenamePrefix + "_pose.txt").c_str()) ;
strm << pose_.rotation() << endl << pose_.translation() ;
}
++c ;
}
grabber.disconnect();
robot_helpers::setServoPowerOff() ;
// exit(1) ;
vector<Affine3d> gripper_to_base, target_to_sensor ;
Affine3d sensor_to_base, sensor_to_gripper ;
cv::Mat cameraMatrix ;
cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
cameraMatrix.at<double>(0, 0) = fx ;
cameraMatrix.at<double>(1, 1) = fy ;
cameraMatrix.at<double>(0, 2) = cx ;
cameraMatrix.at<double>(1, 2) = cy ;
// find_target_motions("grab_", "/home/malasiot/images/clothes/calibration/calib_xtion2/", boardSize, cellSize, cameraMatrix, true, gripper_to_base, target_to_sensor) ;
find_target_motions("grab_", dataFolder, boardSize, cellSize, cameraMatrix, true, gripper_to_base, target_to_sensor) ;
if ( fixedCam )
solveHandEyeFixed(gripper_to_base, target_to_sensor, Tsai, true, sensor_to_base) ;
else
solveHandEyeMoving(gripper_to_base, target_to_sensor, Horaud, false, sensor_to_gripper) ;
certh_libs::createDir(boost::filesystem::path(outFolder).parent_path().string(), true) ;
ofstream ostrm(outFolder.c_str()) ;
if ( fixedCam )
{
ostrm << sensor_to_base.inverse().matrix() ;
// cout << sensor_to_base.inverse().matrix() ;
}
else
{
ostrm << sensor_to_gripper.inverse().matrix() ;
// cout << sensor_to_gripper.inverse().matrix() << endl ;
/*
for(int i=0 ; i<gripper_to_base.size() ; i++)
cout << (gripper_to_base[i] * sensor_to_gripper * target_to_sensor[i]).matrix() << endl ;
*/
}
return 1;
}
void setupSBA(SysSBA &sba)
{
// 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)
// Create a plane containing a wall of points.
Plane plane0, plane1;
plane0.resize(3, 2, 10, 5);
plane1 = plane0;
plane1.translate(0.1, 0.05, 0.0);
plane1.rotate(PI/4.0, 1, 0, 0);
plane1.translate(0.0, 0.0, 7.0);
plane0.rotate(PI/4.0, 1, 0, 0);
plane0.translate(0.0, 0.0, 7.0);
//plane1.translate(0.05, 0.0, 0.05);
// Create nodes and add them to the system.
unsigned int nnodes = 2; // Number of nodes.
double path_length = 2; // Length of the path the nodes traverse.
// Set the random seed.
unsigned short seed = (unsigned short)time(NULL);
seed48(&seed);
for (int 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 0
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 0
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.
sba.addNode(trans, rot, cam_params, false);
}
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;
// Project points into nodes.
addPointAndProjection(sba, plane0.points, 0);
addPointAndProjection(sba, plane1.points, 1);
int offset = plane0.points.size();
Vector3d normal0 = sba.nodes[0].qrot.toRotationMatrix().transpose()*plane0.normal;
Vector3d normal1 = sba.nodes[1].qrot.toRotationMatrix().transpose()*plane1.normal;
printf("Normal: %f %f %f -> %f %f %f\n", plane0.normal.x(), plane0.normal.y(), plane0.normal.z(), normal0.x(), normal0.y(), normal0.z());
printf("Normal: %f %f %f -> %f %f %f\n", plane1.normal.x(), plane1.normal.y(), plane1.normal.z(), normal1.x(), normal1.y(), normal1.z());
for (int i = 0; i < plane0.points.size(); i++)
{
sba.addPointPlaneMatch(0, i, normal0, 1, i+offset, normal1);
Matrix3d covar;
covar << 0.1, 0, 0,
0, 0.1, 0,
0, 0, 0.1;
sba.setProjCovariance(0, i+offset, covar);
sba.setProjCovariance(1, i, covar);
}
// Add noise to node position.
double transscale = 1.0;
double rotscale = 0.1;
// Don't actually add noise to the first node, since it's fixed.
for (int i = 1; i < sba.nodes.size(); i++)
{
Vector4d temptrans = sba.nodes[i].trans;
Quaterniond tempqrot = sba.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();
sba.nodes[i].trans = temptrans;
sba.nodes[i].qrot = tempqrot;
// These methods should be called to update the node.
sba.nodes[i].normRot();
sba.nodes[i].setTransform();
sba.nodes[i].setProjection();
sba.nodes[i].setDr(true);
}
}
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 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 = 0.3; // Baseline
// 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.rotate(PI/4.0, PI/6.0, 1, 0);
middleplane.rotate(PI/4.0, 1, 0, 1);
middleplane.translate(0.0, 0.0, 5.0);
// Create another plane containing a wall of points.
Plane mp2;
mp2.resize(3, 2, 10, 5);
mp2.rotate(0, 0, 0, 1);
mp2.translate(0.0, 0.0, 4.0);
// Create another plane containing a wall of points.
Plane mp3;
mp3.resize(3, 2, 10, 5);
mp3.rotate(-PI/4.0, 1, 0, 1);
mp3.translate(0.0, 0.0, 4.5);
// Vector containing the true point positions.
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(), mp2.points.begin(), mp2.points.end());
normals.insert(normals.end(), mp2.points.size(), mp2.normal);
points.insert(points.end(), mp3.points.begin(), mp3.points.end());
normals.insert(normals.end(), mp3.points.size(), mp3.normal);
// Create nodes and add them to the system.
unsigned int nnodes = 2; // Number of nodes.
double path_length = 1.0; // Length of the path the nodes traverse.
// Set the random seed.
unsigned short seed = (unsigned short)time(NULL);
seed48(&seed);
unsigned int i = 0;
Vector3d inormal0 = middleplane.normal;
Vector3d inormal1 = middleplane.normal;
Vector3d inormal20 = mp2.normal;
Vector3d inormal21 = mp2.normal;
Vector3d inormal30 = mp3.normal;
Vector3d inormal31 = mp3.normal;
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 >= 2)
{
// 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 >= 2)
{
// 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);
// set normal
if (i == 0)
{
inormal0 = rot.toRotationMatrix().transpose() * inormal0;
inormal20 = rot.toRotationMatrix().transpose() * inormal20;
inormal30 = rot.toRotationMatrix().transpose() * inormal30;
}
else
{
inormal1 = rot.toRotationMatrix().transpose() * inormal1;
inormal21 = rot.toRotationMatrix().transpose() * inormal21;
inormal31 = rot.toRotationMatrix().transpose() * inormal31;
}
}
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);
}
// Each point gets added twice
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, proj2dp;
Vector3d proj, projp, pc, pcp, baseline, baselinep;
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();
printf("Normal for Middle Plane: [%f %f %f], index %d -> %d\n", middleplane.normal.x(), middleplane.normal.y(), middleplane.normal.z(), 0, midindex-1);
int nn = points.size();
// Project points into nodes.
for (i = 0; i < points.size(); i++)
{
int k=i;
// Project the point into the node's image coordinate system.
sys.nodes[0].setProjection();
sys.nodes[0].project2im(proj2d, points[k]);
sys.nodes[1].setProjection();
sys.nodes[1].project2im(proj2dp, points[k]);
// Camera coords for right camera
baseline << sys.nodes[0].baseline, 0, 0;
pc = sys.nodes[0].Kcam * (sys.nodes[0].w2n*points[k] - baseline);
proj.head<2>() = proj2d;
proj(2) = pc(0)/pc(2);
baseline << sys.nodes[1].baseline, 0, 0;
pcp = sys.nodes[1].Kcam * (sys.nodes[1].w2n*points[k] - baseline);
projp.head<2>() = proj2dp;
projp(2) = pcp(0)/pcp(2);
// add noise to projections
double prnoise = 0.5; // 0.5 pixels
proj.head<2>() += Vector2d(prnoise*(drand48() - 0.5),prnoise*(drand48() - 0.5));
projp.head<2>() += Vector2d(prnoise*(drand48() - 0.5),prnoise*(drand48() - 0.5));
// 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)
{
// add point cloud shape-holding projections to each node
sys.addStereoProj(0, k, proj);
sys.addStereoProj(1, k+nn, projp);
#ifdef USE_PP
// add exact matches
if (i == 20 || i == 65 || i == 142)
sys.addStereoProj(1, k, projp);
#endif
#ifdef USE_PPL
// add point-plane matches
if (i < midindex)
sys.addPointPlaneMatch(0, k, inormal0, 1, k+nn, inormal1);
else if (i < 2*midindex)
sys.addPointPlaneMatch(0, k, inormal20, 1, k+nn, inormal21);
else
sys.addPointPlaneMatch(0, k, inormal30, 1, k+nn, inormal31);
// sys.addStereoProj(0, k+nn, projp);
// sys.addStereoProj(1, k, proj);
Matrix3d covar;
double cv = 0.01;
covar << cv, 0, 0,
0, cv, 0,
0, 0, cv;
sys.setProjCovariance(0, k+nn, covar);
sys.setProjCovariance(1, k, covar);
#endif
}
else
{
cout << "ERROR! point not in view of nodes" << endl;
//return;
}
}
// Add noise to node position.
double transscale = 2.0;
double rotscale = 0.5;
// 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);
}
}