00001 #include "rb_tracker/StaticEnvironmentFilter.h" 00002 #include "omip_common/OMIPUtils.h" 00003 #include <boost/foreach.hpp> 00004 #include <boost/numeric/ublas/matrix.hpp> 00005 00006 using namespace omip; 00007 00008 using namespace MatrixWrapper; 00009 using namespace BFL; 00010 00011 tf::Transform Eigen2Tf(Eigen::Matrix4f trans) 00012 { 00013 tf::Matrix3x3 btm; 00014 btm.setValue(trans(0,0),trans(0,1),trans(0,2), 00015 trans(1,0),trans(1,1),trans(1,2), 00016 trans(2,0),trans(2,1),trans(2,2)); 00017 tf::Transform ret; 00018 ret.setOrigin(tf::Vector3(trans(0,3),trans(1,3),trans(2,3))); 00019 ret.setBasis(btm); 00020 return ret; 00021 } 00022 00023 tf::Transform Eigen2Tf(Eigen::Matrix4d trans) 00024 { 00025 tf::Matrix3x3 btm; 00026 btm.setValue(trans(0,0),trans(0,1),trans(0,2), 00027 trans(1,0),trans(1,1),trans(1,2), 00028 trans(2,0),trans(2,1),trans(2,2)); 00029 tf::Transform ret; 00030 ret.setOrigin(tf::Vector3(trans(0,3),trans(1,3),trans(2,3))); 00031 ret.setBasis(btm); 00032 return ret; 00033 } 00034 00035 StaticEnvironmentFilter::StaticEnvironmentFilter(double loop_period_ns, FeaturesDataBase::Ptr feats_database, 00036 double environment_motion_threshold) : 00037 RBFilter() 00038 { 00039 this->_loop_period_ns = loop_period_ns; 00040 this->_id = 0; 00041 this->_features_database = feats_database; 00042 this->_estimation_error_threshold = environment_motion_threshold; 00043 00044 this->_tf_epsilon_linear = 1e-8; 00045 this->_tf_epsilon_angular = 1.7e-9; 00046 this->_max_iterations = 100; 00047 00048 this->_pose = Eigen::Matrix4d::Identity(); 00049 this->_velocity = Eigen::Twistd(0.,0.,0.,0.,0.,0); 00050 00051 this->_motion_constraint = NO_MOTION; 00052 00053 this->_computation_type = STATIC_ENVIRONMENT_EKF_TRACKER; 00054 00055 // To avoid segmentation fault because the features are as old as the rigid body 00056 this->_trajectory.push_back(this->_pose); 00057 this->_trajectory.push_back(this->_pose); 00058 this->_trajectory.push_back(this->_pose); 00059 this->_trajectory.push_back(this->_pose); 00060 } 00061 00062 void StaticEnvironmentFilter::Init() 00063 { 00064 RBFilter::Init(); 00065 _pose_covariance.setZero(); 00066 _velocity_covariance.setZero(); 00067 } 00068 00069 void StaticEnvironmentFilter::predictState(double time_interval_ns) 00070 { 00071 if(this->_motion_constraint == NO_MOTION) 00072 { 00073 this->_predicted_pose_vh = _pose; 00074 this->_predicted_pose_bh = _pose; 00075 return; 00076 } 00077 00078 switch(this->_computation_type) 00079 { 00080 00081 case STATIC_ENVIRONMENT_ICP_TRACKER: 00082 { 00083 Eigen::Twistd predicted_delta_pose_vh = (this->_velocity*time_interval_ns/1e9); 00084 this->_predicted_pose_vh = predicted_delta_pose_vh.exp(1e-12).toHomogeneousMatrix()*_pose; 00085 this->_predicted_pose_bh = _pose; 00086 } 00087 break; 00088 case STATIC_ENVIRONMENT_EKF_TRACKER: 00089 { 00090 RBFilter::predictState(time_interval_ns); 00091 } 00092 break; 00093 00094 default: 00095 ROS_ERROR_STREAM_NAMED("StaticEnvironmentFilter.predictState","Wrong StaticEnvironment computation_type!"); 00096 break; 00097 } 00098 } 00099 00100 void StaticEnvironmentFilter::correctState() 00101 { 00102 // The problem is that the StaticEnvironmentFilter is created at the beggining when only one location per feature is available 00103 // There is no way to correct the state with only one location per feature 00104 static bool first_time = true; 00105 if(first_time) 00106 { 00107 this->_trajectory.push_back(this->_pose); 00108 first_time = false; 00109 return; 00110 } 00111 00112 if(this->_motion_constraint == NO_MOTION) 00113 { 00114 this->_velocity = Eigen::Twistd(0,0,0,0,0,0); 00115 this->_trajectory.push_back(_pose); 00116 return; 00117 } 00118 00119 switch(this->_computation_type) 00120 { 00121 00122 case STATIC_ENVIRONMENT_ICP_TRACKER: 00123 { 00124 // I follow the convention of "Introduction to robotics" by Craig 00125 // The first fram is the reference (left upper index) and the second frame is the referred (left lower index) 00126 // se -> static environment 00127 // set -> static environment at time t 00128 // Example: setnext_set_T is describes the frame "static environment at time t" wrt the frame "static environment at time tnext" 00129 tf::Transform set_setnext_Tf; 00130 this->estimateDeltaMotion(this->_supporting_features_ids, set_setnext_Tf); 00131 00132 Eigen::Matrix4d set_setnext_T; 00133 set_setnext_Tf.getOpenGLMatrix(set_setnext_T.data()); 00134 00135 Eigen::Twistd set_setnext_Twist; 00136 TransformMatrix2Twist(set_setnext_T, set_setnext_Twist); 00137 00138 this->_velocity = set_setnext_Twist/(this->_loop_period_ns/1e9); 00139 00140 // Accumulate the new delta into the absolute pose of the static environment wrt the camera 00141 this->_pose = this->_pose*set_setnext_T; 00142 00143 this->_trajectory.push_back(this->_pose); 00144 00145 constrainMotion(); 00146 } 00147 break; 00148 case STATIC_ENVIRONMENT_EKF_TRACKER: 00149 { 00150 RBFilter::correctState(); 00151 00152 constrainMotion(); 00153 } 00154 break; 00155 00156 default: 00157 ROS_ERROR_STREAM_NAMED("StaticEnvironmentFilter.correctState","Wrong StaticEnvironment computation_type!"); 00158 break; 00159 } 00160 } 00161 00162 void StaticEnvironmentFilter::setMotionConstraint(int motion_constraint) 00163 { 00164 this->_motion_constraint = (MotionConstraint)motion_constraint; 00165 } 00166 00167 void StaticEnvironmentFilter::setComputationType(static_environment_tracker_t computation_type) 00168 { 00169 this->_computation_type = computation_type; 00170 } 00171 00172 void StaticEnvironmentFilter::addSupportingFeature(Feature::Id supporting_feat_id) 00173 { 00174 this->_supporting_features_ids.push_back(supporting_feat_id); 00175 00176 Feature::Location predicted_location_velocity; 00177 Feature::Location predicted_location_brake; 00178 FeaturePCLwc predicted_feature_pcl; 00179 // Predict feature location based on velocity hypothesis 00180 00181 predicted_location_velocity = this->_features_database->getFeatureLastLocation(supporting_feat_id); 00182 LocationAndId2FeaturePCLwc(predicted_location_velocity, supporting_feat_id, predicted_feature_pcl); 00183 this->_predicted_measurement_pc_vh->points.push_back(predicted_feature_pcl); 00184 this->_predicted_measurement_map_vh[supporting_feat_id] = predicted_location_velocity; 00185 00186 // Predict feature location based on brake hypothesis (same as last) 00187 predicted_location_brake = this->_features_database->getFeatureLastLocation(supporting_feat_id); 00188 LocationAndId2FeaturePCLwc(predicted_location_brake, supporting_feat_id, predicted_feature_pcl); 00189 this->_predicted_measurement_pc_bh->points.push_back(predicted_feature_pcl); 00190 this->_predicted_measurement_map_bh[supporting_feat_id] = predicted_location_brake; 00191 } 00192 00193 void StaticEnvironmentFilter::estimateDeltaMotion(std::vector<Feature::Id>& supporting_features_ids, tf::Transform& previous_current_Tf) 00194 { 00195 // Prepare the 2 point clouds with the locations of the features in current and previous frames 00196 pcl::PointCloud<pcl::PointXYZ> previous_locations, current_locations; 00197 BOOST_FOREACH(Feature::Id supporting_feat_id, supporting_features_ids) 00198 { 00199 if (this->_features_database->isFeatureStored(supporting_feat_id) && this->_features_database->getFeatureAge(supporting_feat_id) > 2) 00200 { 00201 Feature::Location previous_location = this->_features_database->getFeatureNextToLastLocation(supporting_feat_id); 00202 Feature::Location current_location = this->_features_database->getFeatureLastLocation(supporting_feat_id); 00203 previous_locations.push_back(pcl::PointXYZ(previous_location.get<0>(), previous_location.get<1>() ,previous_location.get<2>())); 00204 current_locations.push_back(pcl::PointXYZ(current_location.get<0>(), current_location.get<1>() ,current_location.get<2>())); 00205 } 00206 } 00207 00208 if (previous_locations.size() ==0) 00209 { 00210 ROS_ERROR_STREAM_NAMED("StaticEnvironmentFilter.estimateDeltaMotion","There are no features supporting the static environment!"); 00211 previous_current_Tf.setIdentity(); 00212 }else{ 00213 iterativeICP(previous_locations, current_locations, previous_current_Tf); 00214 } 00215 } 00216 00217 void StaticEnvironmentFilter::iterativeICP( pcl::PointCloud<pcl::PointXYZ>& previous_locations, 00218 pcl::PointCloud<pcl::PointXYZ>& current_locations, tf::Transform& previous_current_Tf) 00219 { 00220 // initialize the result transform 00221 Eigen::Matrix4f previous_current_T; 00222 previous_current_T.setIdentity(); 00223 00224 // create indices 00225 std::vector<int> previous_indices, current_indices; 00226 for(int i=0; i<previous_locations.size();i++) 00227 { 00228 previous_indices.push_back(i); 00229 current_indices.push_back(i); 00230 } 00231 00232 for (int iteration = 0; iteration < this->_max_iterations; ++iteration) 00233 { 00234 // estimate transformation 00235 Eigen::Matrix4f previous_current_T_new; 00236 _svdecomposer.estimateRigidTransformation (previous_locations, previous_indices, 00237 current_locations, current_indices, 00238 previous_current_T_new); 00239 00240 // transform 00241 pcl::transformPointCloud(previous_locations, previous_locations, previous_current_T_new); 00242 00243 // accumulate incremental tf 00244 previous_current_T = previous_current_T_new * previous_current_T; 00245 00246 // check for convergence 00247 double linear, angular; 00248 00249 previous_current_Tf = Eigen2Tf(previous_current_T); 00250 linear = previous_current_Tf.getOrigin().length(); 00251 double trace = previous_current_Tf.getBasis()[0][0] + previous_current_Tf.getBasis()[1][1] + previous_current_Tf.getBasis()[2][2]; 00252 angular = acos(std::min(1.0, std::max(-1.0, (trace - 1.0)/2.0))); 00253 if (linear < this->_tf_epsilon_linear && angular < this->_tf_epsilon_angular) 00254 { 00255 break; 00256 } 00257 } 00258 } 00259 00260 void StaticEnvironmentFilter::constrainMotion() 00261 { 00262 // The last estimated and unconstrained pose is contained in the variable _pose = cam_setnext_T 00263 00264 // // The previous pose is contained in the vector _trajectory 00265 // Eigen::Matrix4d cam_set_T = _trajectory.at(_trajectory.size() - 2); 00266 00267 // switch(this->_motion_constraint) 00268 // { 00269 // case NO_ROLL_PITCH: 00270 // { 00271 // this->_pose.rz() = cam_set_Twist.rz(); 00272 // this->_pose.ry() = cam_set_Twist.ry(); 00273 00274 // this->_velocity.rz() = 0; 00275 // this->_velocity.ry() = 0; 00276 // } 00277 // break; 00278 // case NO_ROLL_PITCH_TZ: 00279 // { 00280 // this->_pose.rz() = cam_set_Twist.rz(); 00281 // this->_pose.ry() = cam_set_Twist.ry(); 00282 // this->_pose.vz() = cam_set_Twist.vz(); 00283 00284 // this->_velocity.rz() = 0; 00285 // this->_velocity.ry() = 0; 00286 // this->_velocity.vz() = 0; 00287 // } 00288 // break; 00289 // case NO_TRANSLATION: 00290 // { 00291 // this->_pose.vx() = cam_set_Twist.vx(); 00292 // this->_pose.vy() = cam_set_Twist.vy(); 00293 // this->_pose.vz() = cam_set_Twist.vz(); 00294 00295 // this->_velocity.vx() = 0; 00296 // this->_velocity.vy() = 0; 00297 // this->_velocity.vz() = 0; 00298 // } 00299 // break; 00300 // case NO_TRANSLATION_ROLL_YAW: 00301 // { 00302 // this->_pose.vx() = cam_set_Twist.vx(); 00303 // this->_pose.vy() = cam_set_Twist.vy(); 00304 // this->_pose.vz() = cam_set_Twist.vz(); 00305 // this->_pose.rz() = cam_set_Twist.rz(); 00306 // this->_pose.rx() = cam_set_Twist.rx(); 00307 00308 // this->_velocity.vx() = 0; 00309 // this->_velocity.vy() = 0; 00310 // this->_velocity.vz() = 0; 00311 // this->_velocity.rz() = 0; 00312 // this->_velocity.rx() = 0; 00313 // } 00314 // break; 00315 // case NO_ROTATION: 00316 // { 00317 // this->_pose.rx() = cam_set_Twist.rx(); 00318 // this->_pose.ry() = cam_set_Twist.ry(); 00319 // this->_pose.rz() = cam_set_Twist.rz();\ 00320 // this->_velocity.rx() = 0; 00321 // this->_velocity.ry() = 0; 00322 // this->_velocity.rz() = 0; 00323 // } 00324 // break; 00325 // case NO_MOTION: 00326 // { 00327 // this->_pose.rx() = cam_set_Twist.rx(); 00328 // this->_pose.ry() = cam_set_Twist.ry(); 00329 // this->_pose.rz() = cam_set_Twist.rz(); 00330 // this->_pose.vx() = cam_set_Twist.vx(); 00331 // this->_pose.vy() = cam_set_Twist.vy(); 00332 // this->_pose.vz() = cam_set_Twist.vz(); 00333 // this->_velocity.rx() = 0; 00334 // this->_velocity.ry() = 0; 00335 // this->_velocity.rz() = 0; 00336 // this->_velocity.vx() = 0; 00337 // this->_velocity.vy() = 0; 00338 // this->_velocity.vz() = 0; 00339 // } 00340 // break; 00341 // case ROBOT_XY_BASELINK_PLANE: 00342 // { 00343 // // Query the current pose of the camera wrt the baselink frame 00344 // tf::StampedTransform bl_cam_TF; 00345 // Eigen::Matrix4d bl_cam_T; 00346 00347 // try{ 00348 00349 // this->_tf_listener.lookupTransform("/base_link", "/camera_rgb_optical_frame", ros::Time(0), bl_cam_TF); 00350 // bl_cam_TF.getOpenGLMatrix(bl_cam_T.data()); 00351 // } 00352 // catch (tf::TransformException ex) 00353 // { 00354 // ROS_ERROR("waitForTransform in StaticEnvironmentFilter failed!"); 00355 // ROS_ERROR("%s",ex.what()); 00356 // bl_cam_T = Eigen::Matrix4d::Identity(); 00357 // } 00358 00359 // // The matrix of the pose of the static environment to the camera in the next time step 00360 // Eigen::Matrix4d cam_se_T; 00361 // Twist2TransformMatrix(_pose, cam_se_T); 00362 00363 // Eigen::Matrix4d bl_se_T = bl_cam_T * cam_se_T; 00364 00365 // Eigen::Matrix4d bl_se_T_constrained = bl_se_T; 00366 00367 // // Eliminate translation along the z axis of the base link frame 00368 // bl_se_T_constrained(2,3) = bl_cam_T(2,3); 00369 00370 // // Restrict the rotation to be only around the z axis of the base link frame 00371 // Eigen::Quaterniond bl_se_R(bl_se_T.block<3,3>(0,0)); 00372 // Eigen::Vector3d bl_se_RPY = bl_se_R.toRotationMatrix().eulerAngles(2, 1, 0); 00373 00374 // Eigen::Quaterniond bl_cam_R(bl_cam_T.block<3,3>(0,0)); 00375 // Eigen::Vector3d bl_cam_RPY = bl_cam_R.toRotationMatrix().eulerAngles(2, 1, 0); 00376 00377 // Eigen::Vector3d bl_se_RPY_constrained; 00378 00379 // bl_se_RPY_constrained[0] = bl_se_RPY[0]; //Rotation around the baselink_z is the estimated one (unconstrained) 00380 // bl_se_RPY_constrained[1] = bl_cam_RPY[1]; //Rotation around the baselink_y is constrained, must be the same as for the cam 00381 // bl_se_RPY_constrained[2] = bl_cam_RPY[2]; //Rotation around the baselink_x is constrained, must be the same as for the cam 00382 00383 // // Build the constrained rotation matrix 00384 // Eigen::Matrix3f bl_se_R_constrained; 00385 // bl_se_R_constrained = Eigen::AngleAxisf(bl_se_RPY_constrained[0], Eigen::Vector3f::UnitZ()) 00386 // *Eigen::AngleAxisf(bl_se_RPY_constrained[1], Eigen::Vector3f::UnitY()) 00387 // *Eigen::AngleAxisf(bl_se_RPY_constrained[2], Eigen::Vector3f::UnitX()); 00388 00389 // // Assign the rotation part to the constrained transformation matrix 00390 // bl_se_T_constrained.block<3,3>(0,0) = bl_se_R_constrained.cast<double>(); 00391 00392 // // Calculate the constrained pose of the static environment to the camera 00393 // Eigen::Matrix4d cam_se_T_constrained = bl_cam_T.inverse() * bl_se_T_constrained; 00394 // TransformMatrix2Twist(cam_se_T_constrained, _pose); 00395 00396 // // Calculate the constrained velocity of the static environment to the camera 00397 // Eigen::Matrix4d cam_set_T = cam_set_Twist.exp(1e-12).toHomogeneousMatrix(); 00398 // Eigen::Matrix4d set_setnext_T_constrained = cam_set_T.inverse()*cam_se_T_constrained; 00399 // Eigen::Twistd set_setnext_Twist_constrained; 00400 // TransformMatrix2Twist(set_setnext_T_constrained, set_setnext_Twist_constrained); 00401 00402 // _velocity = set_setnext_Twist_constrained / _loop_period_ns/1e9; 00403 // } 00404 // break; 00405 // default: 00406 // break; 00407 00408 // } 00409 00410 // // Reconfigure the variables in the filters 00411 // ColumnVector constrained_state(12); 00412 // constrained_state(1) = this->_pose.vx(); 00413 // constrained_state(2) = this->_pose.vy(); 00414 // constrained_state(3) = this->_pose.vz(); 00415 // constrained_state(4) = this->_pose.rx(); 00416 // constrained_state(5) = this->_pose.ry(); 00417 // constrained_state(6) = this->_pose.rz(); 00418 // constrained_state(7) = this->_velocity.vx(); 00419 // constrained_state(8) = this->_velocity.vy(); 00420 // constrained_state(9) = this->_velocity.vz(); 00421 // constrained_state(10) = this->_velocity.rx(); 00422 // constrained_state(11) = this->_velocity.ry(); 00423 // constrained_state(12) = this->_velocity.rz(); 00424 00425 // this->_ekf_velocity->PostGet()->ExpectedValueSet(constrained_state); 00426 // this->_ekf_brake->PostGet()->ExpectedValueSet(constrained_state); 00427 00428 // // Remove the last estimated pose (unconstrained) from the trajectory vector and replace it with the constrained one 00429 // this->_trajectory.pop_back(); 00430 // this->_trajectory.push_back(this->_pose); 00431 } 00432 00433 //void StaticEnvironmentFilter::constrainMotion(tf::Transform& motion) 00434 //{ 00435 // // **** degree-of-freedom constraints 00436 00437 // switch(this->_motion_constraint) 00438 // { 00439 // case NO_ROLL_PITCH: 00440 // { 00441 // tf::Quaternion q; 00442 // q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation())); 00443 00444 // motion.setRotation(q); 00445 00446 // } 00447 // break; 00448 // case NO_ROLL_PITCH_TZ: 00449 // { 00450 // tf::Quaternion q; 00451 // q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation())); 00452 00453 // tf::Vector3 p(motion.getOrigin().getX(), motion.getOrigin().getY(), 0.0); 00454 00455 // motion.setOrigin(p); 00456 // motion.setRotation(q); 00457 // } 00458 // break; 00459 // case NO_TRANSLATION: 00460 // { 00461 // tf::Vector3 p(0.0,0.0, 0.0); 00462 // motion.setOrigin(p); 00463 // } 00464 // break; 00465 // case NO_TRANSLATION_ROLL_YAW: 00466 // { 00467 // tf::Quaternion q_set; 00468 // tf::Quaternion q(motion.getRotation().x(), 00469 // motion.getRotation().y(), 00470 // motion.getRotation().z(), 00471 // motion.getRotation().w()); 00472 // double roll, pitch, yaw; 00473 // tf::Matrix3x3(q).getRPY(roll, pitch, yaw); 00474 // q_set.setRPY(0.0, pitch, 0.0); 00475 // tf::Vector3 p(0.0,0.0, 0.0); 00476 // motion.setOrigin(p); 00477 // motion.setRotation(q_set); 00478 // } 00479 // break; 00480 // case NO_ROTATION: 00481 // { 00482 // tf::Quaternion q; 00483 // q.setRPY(0.0, 0.0 , 0.0); 00484 // motion.setRotation(q); 00485 // } 00486 // break; 00487 // case NO_MOTION: 00488 // { 00489 // tf::Vector3 p(0.,0., 0.0); 00490 // tf::Quaternion q; 00491 // q.setRPY(0.0, 0.0 , 0.0); 00492 00493 // motion.setOrigin(p); 00494 // motion.setRotation(q); 00495 // } 00496 // break; 00497 // case ROBOT_XY_BASELINK_PLANE: 00498 // { 00499 // // Query the current pose of the camera wrt the baselink frame 00500 // tf::StampedTransform Tf_camt_bl; 00501 // Eigen::Matrix4d T_camt_bl; 00502 // try{ 00503 00504 // this->_tf_listener.lookupTransform("/base_link", "/camera_rgb_optical_frame", ros::Time(0), Tf_camt_bl); 00505 // Tf_camt_bl.getOpenGLMatrix(T_camt_bl.data()); 00506 // } 00507 // catch (tf::TransformException ex) 00508 // { 00509 // ROS_ERROR("waitForTransform in StaticEnvironmentICPFilter failed!"); 00510 // ROS_ERROR("%s",ex.what()); 00511 // T_camt_bl = Eigen::Matrix4d::Identity(); 00512 // getchar(); 00513 // } 00514 00515 // // Apply the estimated delta motion to the pose 00516 // // Verbose! 00517 00518 // Eigen::Matrix4d T_camtnext_camt; 00519 // motion.inverse().getOpenGLMatrix(T_camtnext_camt.data()); 00520 00521 // Eigen::Matrix4d T_camt_camtnext = T_camtnext_camt.inverse(); 00522 00523 // Eigen::Matrix4d T_camtnext_bl = T_camt_bl*T_camt_camtnext.inverse(); 00524 00525 // Eigen::Matrix4d T_camtnext_bl_constrained = T_camtnext_bl; 00526 00527 // // Eliminate translation along the z axis 00528 // T_camtnext_bl_constrained(2,3) = T_camt_bl(2,3); 00529 00530 // // Restrict the rotation to be only around the z axis 00531 // Eigen::Quaterniond R_camtnext_bl(T_camtnext_bl.block<3,3>(0,0)); 00532 // Eigen::Vector3d RPY_camtnext_bl = R_camtnext_bl.toRotationMatrix().eulerAngles(2, 1, 0); 00533 00534 // Eigen::Quaterniond R_camt_bl(T_camt_bl.block<3,3>(0,0)); 00535 // Eigen::Vector3d RPY_camt_bl = R_camt_bl.toRotationMatrix().eulerAngles(2, 1, 0); 00536 00537 // Eigen::Vector3d RPY_camtnext_bl_constrained; 00538 // RPY_camtnext_bl_constrained.x() = RPY_camtnext_bl.x(); //Unconstrained rotation around z 00539 // RPY_camtnext_bl_constrained.y() = RPY_camt_bl.y(); // Constrained rotation around y 00540 // RPY_camtnext_bl_constrained.z() = RPY_camt_bl.z(); // Constrained rotation around x 00541 00542 // // Build the constrained pose 00543 // Eigen::Matrix3f R_camtnext_bl_constrained; 00544 // R_camtnext_bl_constrained = Eigen::AngleAxisf(RPY_camtnext_bl_constrained[0], Eigen::Vector3f::UnitZ()) 00545 // *Eigen::AngleAxisf(RPY_camtnext_bl_constrained[1], Eigen::Vector3f::UnitY()) 00546 // *Eigen::AngleAxisf(RPY_camtnext_bl_constrained[2], Eigen::Vector3f::UnitX()); 00547 00548 // T_camtnext_bl_constrained.block<3,3>(0,0) = R_camtnext_bl_constrained.cast<double>(); 00549 00550 // Eigen::Matrix4d T_camt_camtnext_constrained = ((T_camt_bl.inverse())*T_camtnext_bl_constrained).inverse(); 00551 00552 // Eigen::Matrix4d T_camtnext_camt_constrained = T_camt_camtnext_constrained.inverse(); 00553 00554 // motion = Eigen2Tf(T_camtnext_camt_constrained); 00555 // } 00556 // break; 00557 // default: 00558 // break; 00559 00560 // } 00561 //}