void Correspondence::draw(Viewer &viewer, double radius) { // draw the edge plane viewer.drawEdgePlaneDetails(p,Vec3d(0,0,0),Quaternion(),radius,2,6); // color by camera ID // draw the 3D line if (l == NULL) return; viewer.drawLine(l->_a->getPosition(),l->_b->getPosition(),PINK,6); // color by camera ID // for debug info, draw the projection line in yellow double lambda; Vec3d m = intersectRayPlane(l->_a->getPosition(),p._normal,p._s,p._normal,lambda); viewer.drawLine(l->_a->getPosition(),m,YELLOW,3); m = intersectRayPlane(l->_b->getPosition(),p._normal,p._s,p._normal,lambda); viewer.drawLine(l->_b->getPosition(),m,YELLOW,3); double distance = p.distance(l); }
bool projectRayToGround(const tf::Transformer& listener, const tf::Stamped<tf::Point> camera_ray, Eigen::Vector4d ground_plane, std::string ground_frame, tf::Stamped<tf::Point>* world_point) { tf::StampedTransform camera_transform; // This is a static link, so Time(0) should be fine if (!listener.canTransform(ground_frame, camera_ray.frame_id_, camera_ray.stamp_)) { ROS_INFO("Couldn't transform %s to %s\n",camera_ray.frame_id_.c_str(), ground_frame.c_str()); return false; } listener.lookupTransform(ground_frame, camera_ray.frame_id_,ros::Time(0),camera_transform); tf::Stamped<tf::Point> ground_frame_ray; listener.transformVector(ground_frame, camera_ray, ground_frame_ray); Eigen::Vector3d ray, ray_origin; tf::vectorTFToEigen(ground_frame_ray, ray); tf::vectorTFToEigen(camera_transform.getOrigin(), ray_origin); Eigen::Vector3d ground_v = intersectRayPlane(ray, ray_origin, ground_plane); tf::vectorEigenToTF(ground_v, *world_point); world_point->frame_id_ = ground_frame; world_point->stamp_ = camera_ray.stamp_; return true; }
//----------------------------------------------------------------------------- // Name: collideWithWorld() // Desc: Recursive part of the collision response. This function is the // one who actually calls the collision check on the meshes //----------------------------------------------------------------------------- Fvector msimulator_CollideWithWorld(SCollisionData& cl, Fvector position, Fvector velocity, WORD cnt) { // msimulator_ResolveStuck(cl,position); // do we need to worry ? // if (fsimilar(position.x,target.x,EPS_L)&&fsimilar(position.z,target.z,EPS_L)) if (velocity.magnitude()<EPS_L) { cl.vVelocity.set(0,0,0); return position; } if (cnt>psCollideActDepth) return cl.vLastSafePosition; Fvector ret_pos; Fvector destinationPoint; destinationPoint.add(position,velocity); // reset the collision package we send to the mesh cl.vVelocity.set (velocity); cl.vSourcePoint.set (position); cl.bFoundCollision = FALSE; cl.bStuck = FALSE; cl.fNearestDistance = -1; // Check collision msimulator_CheckCollision (cl); // check return value here, and possibly call recursively if (cl.bFoundCollision == FALSE && !cl.bStuck) { // if no collision move very close to the desired destination. float l = velocity.magnitude(); Fvector V; V.mul(velocity,(l-cl_epsilon)/l); // return the final position ret_pos.add(position,V); // update the last safe position for future error recovery cl.vLastSafePosition.set(ret_pos); return ret_pos; } else { // There was a collision // OK, first task is to move close to where we hit something : Fvector newSourcePoint; // If we are stuck, we just back up to last safe position if (cl.bStuck) { return cl.vLastSafePosition; } // only update if we are not already very close if (cl.fNearestDistance >= cl_epsilon) { Fvector V; V.set(velocity); V.set_length(cl.fNearestDistance-cl_epsilon); newSourcePoint.add(cl.vSourcePoint,V); } else { newSourcePoint.set(cl.vSourcePoint); } // Now we must calculate the sliding plane Fvector slidePlaneOrigin; slidePlaneOrigin.set(cl.vNearestPolygonIntersectionPoint); Fvector slidePlaneNormal; slidePlaneNormal.sub(newSourcePoint,cl.vNearestPolygonIntersectionPoint); // We now project the destination point onto the sliding plane float l = intersectRayPlane(destinationPoint, slidePlaneNormal, slidePlaneOrigin, slidePlaneNormal); // We can now calculate a _new destination point on the sliding plane Fvector newDestinationPoint; newDestinationPoint.mad(destinationPoint,slidePlaneNormal,l); // Generate the slide xr_vector, which will become our _new velocity xr_vector // for the next iteration Fvector newVelocityVector; newVelocityVector.sub(newDestinationPoint, cl.vNearestPolygonIntersectionPoint); // now we recursively call the function with the _new position and velocity cl.vLastSafePosition.set(position); return msimulator_CollideWithWorld(cl, newSourcePoint, newVelocityVector,cnt+1); } }
inline Vec3f intersectRayPlane (const Vec3f& o, const Vec3f& d, const Vec4f& plane ) { float t; return intersectRayPlane(t, o,d,plane); }