OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, int num_primitives)
{
OBBRSS bv;
Matrix3f M;
Vec3f E[3];
Matrix3f::U s[3];
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
axisFromEigen(E, s, bv.obb.axis);
bv.rss.axis[0] = bv.obb.axis[0];
bv.rss.axis[1] = bv.obb.axis[1];
bv.rss.axis[2] = bv.obb.axis[2];
getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.axis, bv.obb.To, bv.obb.extent);
Vec3f origin;
FCL_REAL l[2];
FCL_REAL r;
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axis, origin, l, r);
bv.rss.Tr = origin;
bv.rss.l[0] = l[0];
bv.rss.l[1] = l[1];
bv.rss.r = r;
return bv;
}
RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
{
RSS bv;
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
Matrix3f::U s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
axisFromEigen(E, s, bv.axis);
// set rss origin, rectangle size and radius
Vec3f origin;
FCL_REAL l[2];
FCL_REAL r;
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, origin, l, r);
bv.Tr = origin;
bv.l[0] = l[0];
bv.l[1] = l[1];
bv.r = r;
return bv;
}
void fitn(Vec3f* ps, int n, RSS& bv)
{
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
Matrix3f::U s[3] = {0, 0, 0};
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
axisFromEigen(E, s, bv.axis);
// set rss origin, rectangle size and radius
getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axis, bv.Tr, bv.l, bv.r);
}
void fitn(Vec3f* ps, int n, OBB& bv)
{
Matrix3f M;
Vec3f E[3];
Matrix3f::U s[3] = {0, 0, 0}; // three eigen values
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
axisFromEigen(E, s, bv.axis);
// set obb centers and extensions
getExtentAndCenter(ps, NULL, NULL, NULL, n, bv.axis, bv.To, bv.extent);
}
/// @brief OBB merge method when the centers of two smaller OBB are far away
inline OBB merge_largedist(const OBB& b1, const OBB& b2)
{
OBB b;
Vec3f vertex[16];
computeVertices(b1, vertex);
computeVertices(b2, vertex + 8);
Matrix3f M;
Vec3f E[3];
Matrix3f::U s[3] = {0, 0, 0};
// obb axes
Vec3f& R0 = b.axis[0];
Vec3f& R1 = b.axis[1];
Vec3f& R2 = b.axis[2];
R0 = b1.To - b2.To;
R0.normalize();
Vec3f vertex_proj[16];
for(int i = 0; i < 16; ++i)
vertex_proj[i] = vertex[i] - R0 * vertex[i].dot(R0);
getCovariance(vertex_proj, NULL, NULL, NULL, 16, M);
eigen(M, s, E);
int min, mid, max;
if (s[0] > s[1]) { max = 0; min = 1; }
else { min = 0; max = 1; }
if (s[2] < s[min]) { mid = min; min = 2; }
else if (s[2] > s[max]) { mid = max; max = 2; }
else { mid = 2; }
R1.setValue(E[0][max], E[1][max], E[2][max]);
R2.setValue(E[0][mid], E[1][mid], E[2][mid]);
// set obb centers and extensions
Vec3f center, extent;
getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axis, center, extent);
b.To = center;
b.extent = extent;
return b;
}
OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
{
OBB bv;
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
Matrix3f::U s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
axisFromEigen(E, s, bv.axis);
// set obb centers and extensions
getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, bv.To, bv.extent);
return bv;
}
kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
{
kIOS bv;
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
Matrix3f::U s[3];
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
Vec3f* axis = bv.obb.axis;
axisFromEigen(E, s, axis);
// get centers and extensions
getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, axis, bv.obb.To, bv.obb.extent);
const Vec3f& center = bv.obb.To;
const Vec3f& extent = bv.obb.extent;
FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, center);
// decide k in kIOS
if(extent[0] > kIOS_RATIO * extent[2])
{
if(extent[0] > kIOS_RATIO * extent[1]) bv.num_spheres = 5;
else bv.num_spheres = 3;
}
else bv.num_spheres = 1;
bv.spheres[0].o = center;
bv.spheres[0].r = r0;
if(bv.num_spheres >= 3)
{
FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
Vec3f delta = axis[2] * (r10 * cosA - extent[2]);
bv.spheres[1].o = center - delta;
bv.spheres[2].o = center + delta;
FCL_REAL r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o);
FCL_REAL r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o);
bv.spheres[1].o += axis[2] * (-r10 + r11);
bv.spheres[2].o += axis[2] * (r10 - r12);
bv.spheres[1].r = r10;
bv.spheres[2].r = r10;
}
if(bv.num_spheres >= 5)
{
FCL_REAL r10 = bv.spheres[1].r;
Vec3f delta = axis[1] * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
bv.spheres[3].o = bv.spheres[0].o - delta;
bv.spheres[4].o = bv.spheres[0].o + delta;
FCL_REAL r21 = 0, r22 = 0;
r21 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[3].o);
r22 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[4].o);
bv.spheres[3].o += axis[1] * (-r10 + r21);
bv.spheres[4].o += axis[1] * (r10 - r22);
bv.spheres[3].r = r10;
bv.spheres[4].r = r10;
}
return bv;
}
void fitn(Vec3f* ps, int n, kIOS& bv)
{
Matrix3f M;
Vec3f E[3];
Matrix3f::U s[3] = {0, 0, 0}; // three eigen values;
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
Vec3f* axis = bv.obb.axis;
axisFromEigen(E, s, axis);
getExtentAndCenter(ps, NULL, NULL, NULL, n, axis, bv.obb.To, bv.obb.extent);
// get center and extension
const Vec3f& center = bv.obb.To;
const Vec3f& extent = bv.obb.extent;
FCL_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center);
// decide the k in kIOS
if(extent[0] > kIOS_RATIO * extent[2])
{
if(extent[0] > kIOS_RATIO * extent[1]) bv.num_spheres = 5;
else bv.num_spheres = 3;
}
else bv.num_spheres = 1;
bv.spheres[0].o = center;
bv.spheres[0].r = r0;
if(bv.num_spheres >= 3)
{
FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
Vec3f delta = axis[2] * (r10 * cosA - extent[2]);
bv.spheres[1].o = center - delta;
bv.spheres[2].o = center + delta;
FCL_REAL r11 = 0, r12 = 0;
r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o);
r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o);
bv.spheres[1].o += axis[2] * (-r10 + r11);
bv.spheres[2].o += axis[2] * (r10 - r12);
bv.spheres[1].r = r10;
bv.spheres[2].r = r10;
}
if(bv.num_spheres >= 5)
{
FCL_REAL r10 = bv.spheres[1].r;
Vec3f delta = axis[1] * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
bv.spheres[3].o = bv.spheres[0].o - delta;
bv.spheres[4].o = bv.spheres[0].o + delta;
FCL_REAL r21 = 0, r22 = 0;
r21 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[3].o);
r22 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[4].o);
bv.spheres[3].o += axis[1] * (-r10 + r21);
bv.spheres[4].o += axis[1] * (r10 - r22);
bv.spheres[3].r = r10;
bv.spheres[4].r = r10;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "msf_eval");
ros::Time::init();
enum argIndices{
bagfile = 1,
EVAL_topic = 2,
GT_topic = 3,
singleRunOnly = 4
};
//calibration of sensor to vicon
Eigen::Matrix4d T_BaBg_mat;
//this is the Vicon one - SLAM sensor V0
///////////////////////////////////////////////////////////////////////////////////////////
T_BaBg_mat << 0.999706627053000, -0.022330158354000, 0.005123243528000, -0.060614697387000, 0.022650462142000, 0.997389634278000, -0.068267398302000, 0.035557942651000, -0.003589706237000, 0.068397960288000, 0.997617159323000, -0.042589657349000, 0, 0, 0, 1.000000000000000;
////////////////////////////////////////////////////////////////////////////////////////////
ros::Duration dt(0.0039);
const double timeSyncThreshold = 0.005;
const double timeDiscretization = 10.0; //discretization step for different starting points into the dataset
const double trajectoryTimeDiscretization = 0.049; //discretization of evaluation points (vision framerate for fair comparison)
const double startTimeOffset = 10.0;
if (argc < 4)
{
MSF_ERROR_STREAM("usage: ./"<<argv[0]<<" bagfile EVAL_topic GT_topic [singleRunOnly]");
return -1;
}
bool singleRun = false;
if (argc == 5)
{
singleRun = atoi(argv[singleRunOnly]);
}
if(singleRun){
MSF_WARN_STREAM("Doing only a single run.");
}else{
MSF_WARN_STREAM("Will process the dataset from different starting points.");
}
typedef geometry_msgs::TransformStamped GT_TYPE;
typedef geometry_msgs::PoseWithCovarianceStamped EVAL_TYPE;
// output file
std::stringstream matlab_fname;
std::string path = ros::package::getPath("msf_eval");
matlab_fname << path << "/Matlab/matlab_data" << ros::Time::now().sec << ".m";
std::ofstream outfile(matlab_fname.str().c_str());
std::stringstream poses_EVAL;
std::stringstream poses_GT;
assert(outfile.good());
outfile << "data=[" << std::endl;
ros::Duration startOffset(startTimeOffset);
sm::kinematics::Transformation T_BaBg(T_BaBg_mat); //body aslam to body Ground Truth
// open for reading
rosbag::Bag bag(argv[bagfile], rosbag::bagmode::Read);
// views on topics
rosbag::View view_EVAL(bag, rosbag::TopicQuery(argv[EVAL_topic]));
rosbag::View view_GT(bag, rosbag::TopicQuery(argv[GT_topic]));
//check topics
if (view_EVAL.size() == 0)
{
MSF_ERROR_STREAM("The bag you provided does not contain messages for topic "<<argv[EVAL_topic]);
return -1;
}
if (view_GT.size() == 0)
{
MSF_ERROR_STREAM("The bag you provided does not contain messages for topic "<<argv[GT_topic]);
return -1;
}
//litter console with number of messages
MSF_INFO_STREAM("Reading from "<<argv[bagfile]);
MSF_INFO_STREAM("Topic "<<argv[EVAL_topic]<<", size: "<<view_EVAL.size());
MSF_INFO_STREAM("Topic "<<argv[GT_topic]<<", size: "<<view_GT.size());
//get times of first messages
GT_TYPE::ConstPtr GT_begin = view_GT.begin()->instantiate<GT_TYPE>();
assert(GT_begin);
EVAL_TYPE::ConstPtr POSE_begin = view_EVAL.begin()->instantiate<EVAL_TYPE>();
assert(POSE_begin);
MSF_INFO_STREAM("First GT data at "<<GT_begin->header.stamp);
MSF_INFO_STREAM("First EVAL data at "<<POSE_begin->header.stamp);
while (true) // start eval from different starting points
{
rosbag::View::const_iterator it_EVAL = view_EVAL.begin();
rosbag::View::const_iterator it_GT = view_GT.begin();
// read ground truth
ros::Time start;
// Find start time alignment: set the GT iterater to point to a time larger than the aslam time
while (true)
{
GT_TYPE::ConstPtr trafo = it_GT->instantiate<GT_TYPE>();
assert(trafo);
ros::Time time_GT;
time_GT = trafo->header.stamp + dt;
EVAL_TYPE::ConstPtr pose = it_EVAL->instantiate<EVAL_TYPE>();
assert(pose);
ros::Time time_EKF;
time_EKF = pose->header.stamp;
if (time_EKF >= time_GT)
{
it_GT++;
if (it_GT == view_GT.end())
{
MSF_ERROR_STREAM("Time synchronization failed");
return false;
}
}
else
{
start = time_GT;
MSF_INFO_STREAM("Time synced! GT start: "<<start <<" EVAL start: "<<time_EKF);
break;
}
}
// world frame alignment
sm::kinematics::Transformation T_WaBa;
sm::kinematics::Transformation T_WgBg;
sm::kinematics::Transformation T_WgBg_last;
sm::kinematics::Transformation T_WaWg;
// now find the GT/EKF pairings
int ctr = 0; //how many meas did we add this run?
double ds = 0.0; // distance travelled
ros::Time lastTime(0.0);
MSF_INFO_STREAM("Processing measurements... Current start point: "<<startOffset<<"s into the bag.");
for (; it_GT != view_GT.end(); ++it_GT)
{
GT_TYPE::ConstPtr trafo = it_GT->instantiate<GT_TYPE>();
assert(trafo);
// find closest timestamp
ros::Time time_GT = trafo->header.stamp + dt;
EVAL_TYPE::ConstPtr pose = it_EVAL->instantiate<EVAL_TYPE>();
assert(pose);
ros::Time time_EKF = pose->header.stamp;
bool terminate = false;
//get the measurement close to this GT value
while (time_GT > time_EKF)
{
it_EVAL++;
if (it_EVAL == view_EVAL.end())
{
terminate = true;
MSF_INFO_STREAM("done. All EKF meas processed!");
break;
}
pose = it_EVAL->instantiate<EVAL_TYPE>();
assert(pose);
time_EKF = pose->header.stamp;
}
if (terminate){
break;
}
// add comparison value
if (time_GT - start >= startOffset)
{
T_WaBa = sm::kinematics::Transformation(
Eigen::Vector4d(-pose->pose.pose.orientation.x, -pose->pose.pose.orientation.y,
-pose->pose.pose.orientation.z, pose->pose.pose.orientation.w),
Eigen::Vector3d(pose->pose.pose.position.x, pose->pose.pose.position.y, pose->pose.pose.position.z));
T_WgBg = sm::kinematics::Transformation(
Eigen::Vector4d(-trafo->transform.rotation.x, -trafo->transform.rotation.y, -trafo->transform.rotation.z,
trafo->transform.rotation.w),
Eigen::Vector3d(trafo->transform.translation.x, trafo->transform.translation.y,
trafo->transform.translation.z));
// initial alignment
if (ctr == 0)
{
T_WaWg = (T_WaBa) * T_BaBg * T_WgBg.inverse();
T_WgBg_last = T_WgBg;
}
sm::kinematics::Transformation dT = T_WaBa * (T_WaWg * T_WgBg * T_BaBg.inverse()).inverse();
sm::kinematics::Transformation T_WaBa_gt = (T_WaWg * T_WgBg * T_BaBg.inverse());
T_WaBa_gt = sm::kinematics::Transformation(T_WaBa_gt.q().normalized(), T_WaBa_gt.t());
dT = sm::kinematics::Transformation(dT.q().normalized(), dT.t());
// update integral
if (trafo)
{
ds += (T_WgBg.t() - T_WgBg_last.t()).norm();
// store last GT transformation
T_WgBg_last = T_WgBg;
}
else
{
// too noisy
if ((T_WgBg * T_WgBg_last.inverse()).t().norm() > .1)
{
ds += (T_WgBg.t() - T_WgBg_last.t()).norm();
//MSF_INFO_STREAM((T_WgBg*T_WgBg_last.inverse()).t().norm());
// store last GT transformation
T_WgBg_last = T_WgBg;
}
}
Eigen::Vector3d dalpha;
if (dT.q().head<3>().norm() > 1e-12)
{
dalpha = (asin(dT.q().head<3>().norm()) * 2 * dT.q().head<3>().normalized());
}
else
{
dalpha = 2 * dT.q().head<3>();
}
// g-vector alignment
Eigen::Vector3d e_z_Wg(0, 0, 1);
Eigen::Vector3d e_z_Wa = dT.C() * e_z_Wg;
double dalpha_e_z = acos(std::min(1.0, e_z_Wg.dot(e_z_Wa)));
if (fabs((time_GT - time_EKF).toSec()) < timeSyncThreshold
&& (time_EKF - lastTime).toSec() > trajectoryTimeDiscretization)
{
if (startOffset.toSec() == startTimeOffset)
{
poses_EVAL << T_WaBa.t().transpose() << ";" << std::endl;
poses_GT << T_WgBg.t().transpose() << ";" << std::endl;
}
Eigen::Matrix<double, 6, 6> cov = getCovariance(pose);
outfile << (time_GT - start).toSec() << " "
<< ds << " " << (T_WaBa.t() - T_WaBa_gt.t()).norm() << " " //translation
<< 2 * acos(std::min(1.0, fabs(dT.q()[3]))) << " " << dalpha_e_z << " " //orientation
<<cov(0, 0)<<" "<<cov(1, 1)<<" "<<cov(2, 2)<<" " //position covariances
<<cov(3, 3)<<" "<<cov(4, 4)<<" "<<cov(5, 5)<<";" //orientation covariances
<< std::endl;
lastTime = time_EKF; // remember
}
// count comparisons
ctr++;
}
}
MSF_INFO_STREAM("Added "<<ctr<<" measurement edges.");
//where in the bag should the next eval start
startOffset += ros::Duration(timeDiscretization);
if (ctr == 0 || singleRun) //any new measurements this run?
{
break;
}
}
// cleanup
bag.close();
outfile << "];";
outfile << "poses = [" << poses_EVAL.str() << "];" << std::endl;
outfile << "poses_GT = [" << poses_GT.str() << "];" << std::endl;
outfile.close();
return 0;
}
RBISUpdateInterface * LegOdoCommon::createMeasurement(BotTrans &odo_positionT, BotTrans &odo_deltaT,
int64_t utime, int64_t prev_utime,
int odo_position_status, float odo_delta_status){
BotTrans odo_velT = getTransAsVelocityTrans(odo_deltaT, utime, prev_utime);
Eigen::MatrixXd cov_legodo_use;
LegOdoCommonMode mode_current = mode_;
if ( (mode_current == MODE_POSITION_AND_LIN_RATE) && (!odo_position_status) ){
if (verbose) std::cout << "LegOdo Mode is MODE_POSITION_AND_LIN_RATE but position is not suitable\n";
if (verbose) std::cout << "Falling back to lin rate only for this iteration\n";
mode_current = MODE_LIN_RATE;
}
bool delta_certain = true;
if (odo_delta_status < 0.5){ // low variance, high reliable
delta_certain = true;
}else{ // high variance, low reliable e.g. breaking contact
delta_certain = false;
}
Eigen::MatrixXd cov_legodo;
Eigen::VectorXi z_indices;
getCovariance(mode_current, delta_certain, cov_legodo, z_indices );
if (mode_current == MODE_LIN_AND_ROT_RATE) {
// Apply a linear and rotation rate correction
// This was finished in Feb 2015 but not tested on the robot
Eigen::VectorXd z_meas(6);
double rpy[3];
bot_quat_to_roll_pitch_yaw(odo_deltaT.rot_quat,rpy);
double elapsed_time = ( (double) utime - prev_utime)/1000000;
double rpy_rate[3];
rpy_rate[0] = rpy[0]/elapsed_time;
rpy_rate[1] = rpy[1]/elapsed_time;
rpy_rate[2] = rpy[2]/elapsed_time;
z_meas.head<3>() = Eigen::Map<const Eigen::Vector3d>(odo_velT.trans_vec);
z_meas.tail<3>() = Eigen::Map<const Eigen::Vector3d>(rpy_rate);
return new RBISIndexedMeasurement(z_indices, z_meas, cov_legodo, RBISUpdateInterface::legodo,
utime);
}else if (mode_current == MODE_LIN_RATE) {
return new RBISIndexedMeasurement(z_indices,
Eigen::Map<const Eigen::Vector3d>( odo_velT.trans_vec ), cov_legodo, RBISUpdateInterface::legodo,
utime);
}else if (mode_current == MODE_POSITION_AND_LIN_RATE) {
// Newly added mode
if (verbose) std::cout << "LegOdometry Mode update both xyz position and rate\n";
Eigen::VectorXd z_meas(6);
z_meas.head<3>() = Eigen::Map<const Eigen::Vector3d>(odo_positionT.trans_vec);
z_meas.tail<3>() = Eigen::Map<const Eigen::Vector3d>(odo_velT.trans_vec);
return new RBISIndexedMeasurement(z_indices, z_meas, cov_legodo, RBISUpdateInterface::legodo,
utime);
}else{
std::cout << "LegOdometry Mode not supported, exiting\n";
return NULL;
}
}
