/*-------------------- coord_rmsd ------------------------------------------
* Takes a pairset and two structures and moves coord1 on top of coord2 so
* that the RMSD is minimized. The superimposed structures are returned as
* c1_new and c2_new. If sub_flag is set, the returned structures contain only
* the subset of residues specified by the pairset.
*/
int
coord_rmsd(struct pair_set *const pairset, struct coord *source,
struct coord *target, const int sub_flag, float *rmsd,
struct coord **c1_new, struct coord **c2_new)
{
float rmat[3][3];
size_t tmp_idx = 0;
size_t size, i;
int r, a, b;
struct RPoint translation1, translation2;
int **pairs = pairset->indices;
struct coord *temp_struct_1 =
coord_template(source, coord_size(source));
struct coord *temp_struct_2 =
coord_template(target, coord_size(target));
const char *this_sub = "coord_rmsd";
/* Create temporary structures to hold the aligned parts of the two structures */
temp_struct_1->seq = seq_copy(source->seq);
temp_struct_2->seq = seq_copy(target->seq);
coord_nm_2_a(temp_struct_1);
coord_nm_2_a(temp_struct_2);
if(sub_flag > 4){
for (i = 0; i < pairset->n; i++) {
a = pairs[i][0];
b = pairs[i][1];
if (a != GAP_INDEX && b != GAP_INDEX) {
copy_coord_elem(temp_struct_1, source, tmp_idx, pairs[i][1]);
copy_coord_elem(temp_struct_2, target, tmp_idx, pairs[i][0]);
tmp_idx++;
}
}
}
else{
for (i = 0; i < pairset->n; i++) {
a = pairs[i][0];
b = pairs[i][1];
if (a != GAP_INDEX && b != GAP_INDEX) {
copy_coord_elem(temp_struct_1, source, tmp_idx, pairs[i][0]);
copy_coord_elem(temp_struct_2, target, tmp_idx, pairs[i][1]);
tmp_idx++;
}
}
}
size = tmp_idx;
coord_trim(temp_struct_1, size);
coord_trim(temp_struct_2, size);
/* Determine the center of mass of the two structures and move the CoMs to the
* coordinate origin.
*/
translation1 = calc_CM(size, temp_struct_1->rp_ca);
translation2 = calc_CM(size, temp_struct_2->rp_ca);
coord_trans(&translation1, temp_struct_1, size);
coord_trans(&translation2, temp_struct_2, size);
/* Determine the rotation matrix and apply the rotation to structure 2.
* Then calculate the RMSD
*/
r = lsq_fit(tmp_idx, temp_struct_1->rp_ca, temp_struct_2->rp_ca,
rmat);
if (r == EXIT_FAILURE) {
err_printf(this_sub, "lsq_fit fail\n");
*rmsd = -1;
goto escape;
}
apply_rot(rmat, temp_struct_1);
*rmsd = calc_RMSD(size, temp_struct_1->rp_ca, temp_struct_2->rp_ca);
/* Move the structures' CoMs back to the original CoM of structure 2.*/
translation2.x *= -1;
translation2.y *= -1;
translation2.z *= -1;
/* If only the aligned subset of the structures is needed, translate and
* return the temporary structures.
*/
if (sub_flag%2) {
coord_trans(&translation2, temp_struct_1, size);
coord_trans(&translation2, temp_struct_2, size);
*c1_new = temp_struct_1;
*c2_new = temp_struct_2;
}
/* Otherwise create a copy of the original structures, apply translation
* and rotation to the copies and return those.
*/
else {
coord_destroy(temp_struct_1);
coord_destroy(temp_struct_2);
*c1_new = coord_template(source, coord_size(source));
*c2_new = coord_template(target, coord_size(target));
(*c1_new)->seq = seq_copy(source->seq);
(*c2_new)->seq = seq_copy(target->seq);
for (i = 0; i < coord_size(source); i++) {
copy_coord_elem(*c1_new, source, i, i);
}
for (i = 0; i < coord_size(target); i++) {
copy_coord_elem(*c2_new, target, i, i);
}
coord_trans(&translation1, *c1_new, (*c1_new)->size);
apply_rot(rmat, *c1_new);
coord_trans(&translation2, *c1_new, (*c1_new)->size);
}
return (EXIT_SUCCESS);
escape:
return (EXIT_FAILURE);
}
/*
* ELASTIC_K - space frame elastic stiffness matrix in global coordnates 22oct02
*/
void elastic_K(
double **k, vec3 *xyz, float *r,
double L, double Le,
int n1, int n2,
float Ax, float Asy, float Asz,
float J, float Iy, float Iz, float E, float G, float p,
int shear
){
double t1, t2, t3, t4, t5, t6, t7, t8, t9, /* coord Xformn */
Ksy, Ksz; /* shear deformatn coefficients */
int i, j;
coord_trans ( xyz, L, n1, n2,
&t1, &t2, &t3, &t4, &t5, &t6, &t7, &t8, &t9, p );
for (i=1;i<=12;i++) for (j=1;j<=12;j++) k[i][j] = 0.0;
if ( shear ) {
Ksy = 12.*E*Iz / (G*Asy*Le*Le);
Ksz = 12.*E*Iy / (G*Asz*Le*Le);
} else Ksy = Ksz = 0.0;
k[1][1] = k[7][7] = E*Ax / Le;
k[2][2] = k[8][8] = 12.*E*Iz / ( Le*Le*Le*(1.+Ksy) );
k[3][3] = k[9][9] = 12.*E*Iy / ( Le*Le*Le*(1.+Ksz) );
k[4][4] = k[10][10] = G*J / Le;
k[5][5] = k[11][11] = (4.+Ksz)*E*Iy / ( Le*(1.+Ksz) );
k[6][6] = k[12][12] = (4.+Ksy)*E*Iz / ( Le*(1.+Ksy) );
k[5][3] = k[3][5] = -6.*E*Iy / ( Le*Le*(1.+Ksz) );
k[6][2] = k[2][6] = 6.*E*Iz / ( Le*Le*(1.+Ksy) );
k[7][1] = k[1][7] = -k[1][1];
k[12][8] = k[8][12] = k[8][6] = k[6][8] = -k[6][2];
k[11][9] = k[9][11] = k[9][5] = k[5][9] = -k[5][3];
k[10][4] = k[4][10] = -k[4][4];
k[11][3] = k[3][11] = k[5][3];
k[12][2] = k[2][12] = k[6][2];
k[8][2] = k[2][8] = -k[2][2];
k[9][3] = k[3][9] = -k[3][3];
k[11][5] = k[5][11] = (2.-Ksz)*E*Iy / ( Le*(1.+Ksz) );
k[12][6] = k[6][12] = (2.-Ksy)*E*Iz / ( Le*(1.+Ksy) );
#ifdef MATRIX_DEBUG
save_dmatrix ( "ke", k, 1,12, 1,12, 0, "w" ); /* element elastic stiffness matrix */
#endif
atma(t1,t2,t3,t4,t5,t6,t7,t8,t9, k, r[n1],r[n2]); /* globalize */
/* check and enforce symmetry of elastic element stiffness matrix */
for(i=1; i<=12; i++){
for (j=i+1; j<=12; j++){
if( k[i][j] != k[j][i] ) {
if(fabs(k[i][j]/k[j][i]-1.0) > 1.0e-6
&&(fabs(k[i][j]/k[i][i]) > 1e-6
|| fabs(k[j][i]/k[i][i]) > 1e-6
)
){
fprintf(stderr,"elastic_K: element stiffness matrix not symetric ...\n" );
fprintf(stderr," ... k[%d][%d] = %15.6e \n",i,j,k[i][j] );
fprintf(stderr," ... k[%d][%d] = %15.6e ",j,i,k[j][i] );
fprintf(stderr," ... relative error = %e \n", fabs(k[i][j]/k[j][i]-1.0) );
fprintf(stderr," ... element matrix saved in file 'kt'\n");
save_dmatrix ( "kt", k, 1,12, 1,12, 0, "w" );
}
k[i][j] = k[j][i] = 0.5 * ( k[i][j] + k[j][i] );
}
}
}
#ifdef MATRIX_DEBUG
save_dmatrix ( "ket", k, 1,12, 1,12, 0, "w" ); /* transformed element matx */
#endif
}
/*
* GEOMETRIC_K - space frame geometric stiffness matrix, global coordnates 20dec07
*/
void geometric_K(
double **k, vec3 *xyz, float *r,
double L, double Le,
int n1, int n2, float Ax, float Asy, float Asz, float J,
float Iy, float Iz, float E, float G, float p, double T,
int shear
){
double t1, t2, t3, t4, t5, t6, t7, t8, t9; /* coord Xformn */
double **kg;
double Ksy,Ksz,Dsy,Dsz; /* shear deformation coefficients */
int i, j;
coord_trans(
xyz, L, n1, n2, &t1, &t2, &t3, &t4, &t5, &t6, &t7, &t8, &t9, p
);
kg = dmatrix(1,12,1,12);
for (i=1;i<=12;i++) for (j=1;j<=12;j++) kg[i][j] = 0.0;
if ( shear ) {
Ksy = 12.*E*Iz / (G*Asy*Le*Le);
Ksz = 12.*E*Iy / (G*Asz*Le*Le);
Dsy = (1+Ksy)*(1+Ksy);
Dsz = (1+Ksz)*(1+Ksz);
} else{
Ksy = Ksz = 0.0;
Dsy = Dsz = 1.0;
}
kg[1][1] = kg[7][7] = 0.0; // T/L;
kg[2][2] = kg[8][8] = T/L*(1.2+2.0*Ksy+Ksy*Ksy)/Dsy;
kg[3][3] = kg[9][9] = T/L*(1.2+2.0*Ksz+Ksz*Ksz)/Dsz;
kg[4][4] = kg[10][10] = T/L*J/Ax;
kg[5][5] = kg[11][11] = T*L*(2.0/15.0+Ksz/6.0+Ksz*Ksz/12.0)/Dsz;
kg[6][6] = kg[12][12] = T*L*(2.0/15.0+Ksy/6.0+Ksy*Ksy/12.0)/Dsy;
kg[1][7] = kg[7][1] = 0.0; // -T/L;
kg[5][3] = kg[3][5] = kg[11][3] = kg[3][11] = -T/10.0/Dsz;
kg[9][5] = kg[5][9] = kg[11][9] = kg[9][11] = T/10.0/Dsz;
kg[6][2] = kg[2][6] = kg[12][2] = kg[2][12] = T/10.0/Dsy;
kg[8][6] = kg[6][8] = kg[12][8] = kg[8][12] = -T/10.0/Dsy;
kg[4][10] = kg[10][4] = -kg[4][4];
kg[8][2] = kg[2][8] = -T/L*(1.2+2.0*Ksy+Ksy*Ksy)/Dsy;
kg[9][3] = kg[3][9] = -T/L*(1.2+2.0*Ksz+Ksz*Ksz)/Dsz;
kg[11][5] = kg[5][11] = -T*L*(1.0/30.0+Ksz/6.0+Ksz*Ksz/12.0)/Dsz;
kg[12][6] = kg[6][12] = -T*L*(1.0/30.0+Ksy/6.0+Ksy*Ksy/12.0)/Dsy;
#ifdef MATRIX_DEBUG
save_dmatrix ( "kg", kg, 1,12, 1,12, 0, "w" ); /* element geom. stiffness matrix */
#endif
atma(t1,t2,t3,t4,t5,t6,t7,t8,t9, kg, r[n1],r[n2]); /* globalize */
/* check and enforce symmetry of geometric element stiffness matrix */
for(i=1; i<=12; i++){
for (j=i+1; j<=12; j++){
if( kg[i][j] != kg[j][i] ) {
if(fabs(kg[i][j]/kg[j][i]-1.0) > 1.0e-6
&&(fabs(kg[i][j]/kg[i][i]) > 1e-6
|| fabs(kg[j][i]/kg[i][i]) > 1e-6
)
){
fprintf(stderr,"geometric_K element stiffness matrix not symetric ...\n" );
fprintf(stderr," ... kg[%d][%d] = %15.6e \n",i,j,kg[i][j] );
fprintf(stderr," ... kg[%d][%d] = %15.6e ",j,i,kg[j][i] );
fprintf(stderr," ... relative error = %e \n", fabs(kg[i][j]/kg[j][i]-1.0) );
fprintf(stderr," ... element matrix saved in file 'kg'\n");
save_dmatrix ( "kg", kg, 1,12, 1,12, 0, "w" );
}
kg[i][j] = kg[j][i] = 0.5 * ( kg[i][j] + kg[j][i] );
}
}
}
#ifdef MATRIX_DEBUG
save_dmatrix ( "kgt", kg, 1,12, 1,12, 0, "w" ); /* transformed element matx */
#endif
/* add geometric stiffness matrix to elastic stiffness matrix ... */
for (i=1; i<=12; i++) for (j=1; j<=12; j++) k[i][j] += kg[i][j];
free_dmatrix(kg,1,12,1,12);
}
void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv_bridge::CvImageConstPtr& cv_ptr)
{
bool publish_images = (image_pub.getNumSubscribers() != 0);
bool publish_viz = (viz_pub.getNumSubscribers() != 0);
bool publish_points = (points_pub.getNumSubscribers() != 0);
bool publish_poses = (poses_pub.getNumSubscribers() != 0);
bool publish_trans_poses = (trans_poses_pub.getNumSubscribers() != 0);
if (!publish_images && !publish_viz && !publish_points && !publish_poses && !publish_trans_poses) return;
// prepare particle markers
visualization_msgs::Marker marker, transformed_marker;
if (publish_viz)
{
marker.header = header;
marker.header.frame_id = "/base_link";
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.color.a = 1;
marker.id = 0;
transformed_marker = marker;
marker.ns = "points";
marker.ns = "poses";
marker.color.r = 1;
marker.color.g = 0;
marker.color.b = 0;
transformed_marker.color.r = 0;
transformed_marker.color.g = 1;
transformed_marker.color.b = 0;
}
// prepare image outpu
cv::Mat output_image;
if (publish_images)
output_image = cv_ptr->image.clone();
geometry_msgs::PoseArray pose_array, trans_pose_array;
whycon::PointArray point_array;
// go through detected targets
for (int i = 0; i < system->targets; i++) {
const cv::CircleDetector::Circle& circle = system->get_circle(i);
cv::LocalizationSystem::Pose pose = system->get_pose(circle);
cv::LocalizationSystem::Pose trans_pose = system->get_transformed_pose(circle);
cv::Vec3f coord = pose.pos;
cv::Vec3f coord_trans = trans_pose.pos;
// draw each target
if (publish_images) {
std::ostringstream ostr;
ostr << std::fixed << std::setprecision(2);
ostr << coord_trans << " " << i;
circle.draw(output_image, ostr.str(), cv::Vec3b(0,255,255));
}
if (publish_viz) {
geometry_msgs::Point marker_point;
marker_point.x = coord(0);
marker_point.y = coord(1);
marker_point.z = coord(2);
marker.points.push_back(marker_point);
if (system->axis_set) {
marker_point.x = coord_trans(0);
marker_point.y = coord_trans(1);
marker_point.z = coord_trans(2);
transformed_marker.points.push_back(marker_point);
}
}
if (publish_trans_poses) {
geometry_msgs::Pose p;
p.position.x = trans_pose.pos(0);
p.position.y = trans_pose.pos(1);
p.position.z = trans_pose.pos(2);
p.orientation = tf::createQuaternionMsgFromRollPitchYaw(trans_pose.rot(0), trans_pose.rot(1), trans_pose.rot(2));
trans_pose_array.poses.push_back(p);
}
if (publish_poses) {
geometry_msgs::Pose p;
p.position.x = pose.pos(0);
p.position.y = pose.pos(1);
p.position.z = pose.pos(2);
p.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, pose.rot(0), pose.rot(1));
pose_array.poses.push_back(p);
}
if (publish_points) {
geometry_msgs::Point p;
p.x = circle.x;
p.y = circle.y;
p.z = 0;
point_array.points.push_back(p);
}
}
if (publish_viz) {
viz_pub.publish(marker);
if (system->axis_set) viz_pub.publish(transformed_marker);
}
if (publish_images) {
cv_bridge::CvImage output_image_bridge = *cv_ptr;
output_image_bridge.image = output_image;
image_pub.publish(output_image_bridge);
}
if (publish_poses) {
pose_array.header = header;
pose_array.header.frame_id = "/base_link";
poses_pub.publish(pose_array);
}
if (publish_trans_poses) {
trans_pose_array.header = header;
trans_pose_array.header.frame_id = "/base_link";
trans_poses_pub.publish(trans_pose_array);
}
if (publish_points) {
point_array.header = header;
point_array.header.frame_id = "/base_link";
points_pub.publish(point_array);
}
}
