System and method for trailer pose estimation
The present teaching relates to method, system, medium, and implementations for trailer pose estimation. At least a dimension of a trailer connected to a vehicle is obtained where the vehicle and the trailer have respective opposing surfaces facing one another. A first set of feature points is identified from a first image of a target present on one the opposing surfaces acquired at a first time instant by a sensor deployed on the other the opposing surfaces. A second set of corresponding feature points is identified from a second image of the target acquired at a subsequent time instant by the sensor. Based on the first and second sets of feature points, a first transformation is determined and used to compute a second transformation that the trailer undergoes from the first to the second time instants. A pose of the trailer at the second time instant is estimated based on the second transformation and the dimension.
1. A system comprising:
at least one processor; and
a memory storing instructions that, when executed by the at least one processor, cause the system to perform operations comprising:
producing a plurality of poses of a trailer for a plurality of times based on repeated identification of sets of feature points and repeated estimation of the plurality of poses over the plurality of times, wherein producing a pose of the trailer comprises:
identifying a first set of feature points from a first image of a fiducial marker present on a surface of a vehicle or a surface of a trailer acquired at a first time by a sensor deployed on the other of the surface of the vehicle or the surface of the trailer;
identifying a second set of feature points from a second image of the fiducial marker acquired at a second time by the sensor, each feature point from the second set of feature points corresponding to one feature point from the first set of feature points; and
estimating the pose of the trailer at the second time based on the first set of feature points, the second set of feature points, and a dimension of the trailer.
2. The system of claim 1 , the operations further comprising:
determining a first transformation based on the first set of feature points and the second set of feature points; and
computing a second transformation that the trailer undergoes between the first time and the second time in accordance with the first transformation, wherein the estimating the pose of the trailer is based on the second transformation.
3. The system of claim 1 , wherein the pose of the trailer is represented by one or more coordinates that correspond to one or more points located on the trailer.
4. The system of claim 1 , the operations further comprising:
controlling the vehicle based on the pose of the trailer at the second time.
5. The system of claim 1 , wherein the first time corresponds to at least one of:
an initialization time where the vehicle and the trailer are aligned in a straight line, and
a time prior to the second time, the pose at the second time representing a change in the pose of the trailer as compared to the first time.
6. The system of claim 1 , wherein the estimating the pose of the trailer is based on planar homography.
7. The system of claim 1 , wherein
the first set of feature points include at least one of: a corner point, a 90 degree concave point, a 90 degree convex point, and a symmetric feature.
8. A computer-implemented method comprising:
producing, by a computing system, a plurality of poses of a trailer for a plurality of times based on repeated identification of sets of feature points and repeated estimation of the plurality of poses over the plurality of times, wherein producing a pose of the trailer comprises:
identifying, by the computing system, a first set of feature points from a first image of a fiducial marker present on a surface of a vehicle or a surface of a trailer acquired at a first time by a sensor deployed on the other of the surface of the vehicle or the surface of the trailer;
identifying, by the computing system, a second set of feature points from a second image of the fiducial marker acquired at a second time by the sensor, each feature point from the second set of feature points corresponds to one feature point from the first set of feature points; and
estimating, by the computing system, the pose of the trailer at the second time based on the first set of feature points, the second set of feature points, and a dimension of the trailer.
9. The computer-implemented method of claim 8 , further comprising:
controlling, by the computing system, the vehicle based on the pose of the trailer at the second time.
10. The computer-implemented method of claim 8 , further comprising:
controlling, by the computing system, based on the pose of the trailer at the second time, the vehicle to ensure that the trailer is in a lane that corresponds to a lane that the vehicle is.
11. The computer-implemented method of claim 8 ,
wherein the fiducial marker has known visual characteristics.
12. The computer-implemented method of claim 8 , further comprising:
wherein the estimating the pose is based on discrepancies between the first set of feature points and the second set of feature points.
13. The computer-implemented method of claim 8 , wherein the
first set of feature points include at least one of: a corner point, a 90 degree concave point, a 90 degree convex point, and a symmetric feature.
14. The computer-implemented method of claim 8 , wherein the first time corresponds to at least one of:
an initialization time where the vehicle and the trailer are aligned in a straight line, and
a time prior to the second time, the pose at the second time representing a change in the pose of the trailer as compared to the first time.
15. A non-transitory computer readable medium including instructions that, when executed by at least one hardware processor of a computing system, cause the computing system to perform operations comprising:
producing a plurality of poses of a trailer for a plurality of times based on repeated identification of sets of feature points and repeated estimation of the plurality of poses over the plurality of times, wherein producing a pose of the trailer comprises:
identifying a first set of feature points from a first image of a fiducial marker present on a surface of a vehicle or a surface of a trailer acquired at a first time by a sensor deployed on the other of the surface of the vehicle or the surface of the trailer;
identifying a second set of feature points from a second image of the fiducial marker acquired at a second time by the sensor, each feature point from the second set of feature points corresponds to one feature point from the first set of feature points;
estimating the pose of the trailer at the second time based on the first set of feature points, the second set of feature points, and a dimension of the trailer.
16. The non-transitory computer readable medium of claim 15 , further comprising:
controlling the vehicle based on the pose of the trailer at the second time.
17. The non-transitory computer readable medium of claim 15 , further comprising:
controlling, based on the pose of the trailer at the second time, the vehicle to ensure that the trailer is in a lane that corresponds to a lane that the vehicle is.
18. The non-transitory computer readable medium of claim 15 , wherein the estimating is based on discrepancies between the first set of feature points and the second set of feature points.
19. The non-transitory computer readable medium of claim 15 , wherein the first set of feature points include at least one of: a corner point, a 90 degree concave point, a 90 degree convex point, and a symmetric feature.
20. The non-transitory computer readable medium of claim 15 , wherein the first time corresponds to at least one of:
an initialization time where the vehicle and the trailer are aligned in a straight line, and
a time prior to the second time, the pose at the second time representing a change in the pose of the trailer as compared to the first time.