-
Notifications
You must be signed in to change notification settings - Fork 110
/
main.go
77 lines (68 loc) · 2.11 KB
/
main.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
// Given at least 4 corresponding points, and the intrinsic matrices of both cameras, computes
// the rigid transform (rotation + translation) that would be the extrinsic transformation
// from camera 1 to camera 2.
// rimage/transform/data/example_extrinsic_calib.json has an example input file.
// $./extrinsic_calibration -conf=/path/to/input/file
package main
import (
"encoding/json"
"flag"
"fmt"
"io"
"os"
"github.com/edaniels/golog"
"github.com/pkg/errors"
"go.viam.com/utils"
"go.viam.com/rdk/rimage/transform"
"go.viam.com/rdk/spatialmath"
)
func main() {
confPtr := flag.String("conf", "", "path of configuration for extrinsic parameter finding")
flag.Parse()
logger := golog.NewLogger("extrinsic_calibration")
calibrate(*confPtr, logger)
os.Exit(0)
}
func calibrate(conf string, logger golog.Logger) {
cfg, err := readConfig(conf)
if err != nil {
logger.Fatal(err)
}
// set up the optimization problem
problem, err := transform.BuildExtrinsicOptProblem(cfg)
if err != nil {
logger.Fatal(err)
}
// solve the problem
pose, err := transform.RunPinholeExtrinsicCalibration(problem, logger)
// print result to output stream
logger.Infof("\nrotation:\n%v\ntranslation:\n%.3f\n", printRot(pose.Orientation()), pose.Point())
if err != nil {
logger.Fatal(err)
}
}
func printRot(o spatialmath.Orientation) string {
final := o.RotationMatrix()
r1, r2, r3 := final.Row(0), final.Row(1), final.Row(2)
w1 := fmt.Sprintf("⸢ %.3f %.3f %.3f ⸣\n", r1.X, r1.Y, r1.Z)
w2 := fmt.Sprintf("| %.3f %.3f %.3f |\n", r2.X, r2.Y, r2.Z)
w3 := fmt.Sprintf("⸤ %.3f %.3f %.3f ⸥", r3.X, r3.Y, r3.Z)
return w1 + w2 + w3
}
func readConfig(cfgPath string) (*transform.ExtrinsicCalibrationConfig, error) {
f, err := os.Open(cfgPath) //nolint:gosec
if err != nil {
return nil, errors.Wrap(err, fmt.Sprintf("path=%q", cfgPath))
}
defer utils.UncheckedErrorFunc(f.Close)
byteJSON, err := io.ReadAll(f)
if err != nil {
return nil, err
}
conf := &transform.ExtrinsicCalibrationConfig{}
err = json.Unmarshal(byteJSON, conf)
if err != nil {
return nil, errors.Wrap(err, "error parsing byte array ")
}
return conf, nil
}